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Abstract 


While performing complex assembly tasks or moving about in space, a space robot 
should minimize the amount of propellant consumed. This thesis comprises an analytical 
and experimental study of space robot locomotion and orientation without the use of 
thrusters. The goal of this research is to design a robust control paradigm that will perform 
thrusterless locomotion between two points on a structure, and to implement this paradigm 
on an experimental robot. 

A two-arm free-flying robot has been constructed which floats on a cushion of air to 
simulate in two dimensions the drag-free, zero-g environment of space. The robot can 
impart momentum to itself by pushing off - from an external structure in a coordinated 
two-arm maneuver, and can then reorient itself by activating a momentum wheel. 

The controller design consists of two parts: a high-level strategic controller and a low- 
level dynamic controller. The strategic controller, implemented as a finite-state machine, 
monitors the state of the system and switches control laws asynchronously, based on discrete 
events. Different specific control laws are implemented depending upon the configuration of 
the system, the number of degrees of freedom, and the desired task. The dynamic controller 
consists of a system of estimators, control laws, trajectory generators, and filters. For 
example, whenever both arms are grasping an external structure, the strategic controller 
installs a momentum controller which causes the linear and angular momentum of the 
system to follow desired trajectories. 

The control paradigm has been verified experimentally by commanding the robot to 
push off from a structure with both arms, rotate 180 degrees while translating freely, 
and then catch itself on another structure. This method, based on the idea of computed 
torque , provides a linear feedback law in momentum and its derivatives for a system of 
rigid bodies. By controlling momentum, a configuration-independent quantity, the robot 
can leap precisely from one place to another, while accounting for nonlinear forces and 
changing kinematic constraints. It is believed that this design approach can be easily 
extended to three dimensions and to more complex robot configurations. 
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Chapter 1 


Introduction 

This dissertation describes the LEAP (Locomotion Enhancement via Arm Push-off) project, 
conducted at The Stanford University Aerospace Robotics Laboratory (ARL) during the 
period 1986-1990. 

1.1 Motivation 

Much of the recent work in robotics has assumed a priori that the desired task is within the 
workspace of the robot. This assumption is clearly evident for all fixed-base robots, whether 
they be bolted to the factory floor or to the cargo bay of the space shuttle. A fixed-base 
robot is ideally suited to perform repetitive tasks in a highly structured environment. 

Unfortunately, this is not the situation in space, where the environment is unstructured 
and the tasks are varied. For example, building or repairing a space-station requires the 
ability to perform a myriad of different tasks at different locations. Many missions that 
the astronauts are called upon to perform require mobility, such as repairing a damaged 
satellite. Therefore, a space robot must be mobile to perform different tasks at various 
locations. If space robots are to be mobile, then one must ask the question: “What are 
good ways for a robot to move from one place to another in space” . 

The ways a robot can move through space can be divided into two types: Those that 
use propulsion as the primary means for imparting momentum to the robot, and those 
that do not. For these two types, this section will briefly examine the issues involved in 
imparting momentum to a space vehicle and reorienting it. 
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Thrusters as a primary means of locomotion Historically, thrusters have been the 
only method employed to relocate space vehicles. The reason for their exclusive use is 
because expelling propellant is the only way for a totally isolated vehicle to impart mo- 
mentum to itself. This capability comes with three disadvantages, namely: Propellant is a 
finite resource that must be transported into space; the propellant may impact or contam- 
inate the surrounding environment, damaging sensitive equipment; and use of nonlinear 
on-off thrusters limits the degree of achievable precision in pointing and positioning. 

On the other hand, in a multi-body environment, one can reduce or eliminate thrusters 
by pushing off from and landing on different structures. Thrusters then need only be 
employed for mid-course correction or during emergency situations, thereby reducing the 
total amount of propellant consumed. This is a' very natural and intuitive approach to 
space locomotion, and is the way humans move around in space. 

Thrusterless Attitude Control Thrusterless attitude control schemes use conservation 
of angular momentum to change orientation without imparting angular momentum to the 
system. This principle was first discovered by Newton [23] around 1666. One of the earlist 
references for using this principle in spacecraft orientation is due to Hohmann [12] in 1925. 
Hohmann advocates using only one thruster on a spacecraft to save weight. To demonstrate 
that a single thruster can be oriented in any direction, there is a picture of two astronauts 
crawling along a circular ladder to change the spacecraft’s orientation. 

Since the early 70’s, satellites have used momentum wheels and magnetic coils to control 
spacecraft attitude [38], While these techniques can control only the attitude (and not the 
location) of the vehicle, they do so with the advantage of converting electrical energy 1 
into torque without using gas propellants. Longman [20] explores the possibility of using 
reaction wheels on a space vehicle with a robotic arm to stabilize vehicle attitude only. In 
this way, the base of the robot arm can be treated as inertially fixed, although Longman 
does not compensate for the linear motion of the base due to the motion of the arm. 

A unified approach to space robotic locomotion needs to address two issues: controlling 
the location of the mass center of the robot, and controlling the orientation about the mass 
center of the robot. The first issue involves controlling linear momentum, whereas the 

1 Electrical energy is obt ain able from the sun by photo voltaic cells, and is therefore a replenishable 


resource 
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second issue involves controlling the geometric configuration of the robot. These issues will 
be dealt with in detail throughout this thesis. 

1.2 Research Goals 

The goal of this project is to study the dynamics and control issues involved in thrusterless 
locomotion, and to demonstrate experimentally that a free-flying multi-arm robot can 
accurately reposition/reorient itself while pushing off from another body (e.g. a space 
station) rather than using thrusters. The aim is not to construct a space qualified robot, 
but rather to demonstrate in two dimensions in the laboratory a design philosophy that 
will easily extend to three dimensions in space. 

More specifically, the goals of this project are to: 

• Develop a control paradigm and simple algorithms that aid in thrusterless locomotion. 

• Achieve smooth switching between control laws during abrupt changes in kinematic 
configuration. A challenging aspect of this project is to control a nonlinear plant 
with changing degrees of freedom. 

• Construct an autonomous free-flying multi-arm robot that simulates the zero-g drag- 
free environment of space. 

• Verify the design experimentally. 

The Experiment Figure 1.1 depicts the conceptual goal of this research: to move the 
robot accurately from one place to another without using thrusters. To demonstrate faith- 
fully the drag-free zero-g space environment, a second-generation air cushion vehicle (ACV) 
was designed and built, with many ideas drawn from previous ACV experiments done at 
Stanford [1]. These vehicles float on a cushion or air on a very flat granite table. Unlike 
previous ACV’s, our current versions have two SCARA 2 arms to allow cooperative manip- 
ulation. A photograph of the experimental hardware is shown in Fig 1.2. At the end of 
each arm is a small robotic hand that can grasp a cylindrical bar affixed to the side of a 
granite table. The hand is instrumented with force sensors in its palm and pneumatically 

2 SCARA is an acronym for Selective Compliance Assembly Robot Arm. Such robot arms are designed 
to be very stiff in the vertical axis while being very compliant or easy to move in the horizontal plane L 21 J- 
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Figure 1.1: Research Objective 

The robot can push off from one end of the table, rotate, and catch itself 
at the other end without using thrusters. 


actuated finger. There is also a photo emitter/detector pair located between the palm and 
finger to sense the bar. Located in the center of the vehicle is a momentum wheel to en- 
able turning while in the free-floating configuration. In the free-floating configuration, the 
robot possesses eight degrees of freedom. With both arms grasping the bar (closed-chain 
configuration), the number of degrees of freedom drops to four. 

1.3 Summary of Results 

1.3.1 System Capabilities 

The ability for the robot to position itself using only electric motors has been demonstrated. 
During the three phases of the LEAP maneuver (Push-off, Coast, Docking), a high level 
strategic controller monitors the system to insure smooth transitions between the phases. 
While the robot is grasping the bar, the robot can follow trajectories in “momentum” space 
with a decoupled closed-kinematic-chain controller. 

When the bar is released, the robot responds to changes in the configuration and realizes 
that linear momentum is no longer controllable. Using a combination of bang-bang and 
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Figure 1.2: Experimental System 

Overall view of the experimental setup. The two-arm robot Boats on an 
air cushion to simulate the space environment. The vehicle is autonomous, 
and has onboard power and high-pressure gas. Two robotic hands grasp 
the bar before and during push-off. 
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joint PD control, the free-floating robot can orient itself to any desired angle using a 
momentum wheel. 

Finally, when two grasp points are encountered, the control laws are changed yet again 
to account for the discontinuous change in plant dynamics. During docking, the robot 
performs a soft-landing by bringing the momentum of the system to zero exponentially. 

The operator only commands a desired launch direction and catch angle. All the other 
state dependent parameters are determined by the strategic controller. The system also 
possesses onboard error detection logic. Upon determining that an actuator has failed, the 
strategic controller attempts to put the robot into a safe state. 

1.3.2 Contributions 

This research makes the following contributions to the field of automatic control, in the 
particular context of free-flying robots. 

• A new approach to using momentum control has been developed. Specifically, this 
approach, called system momentum control , allows the following: 

— Cause a system of rigid bodies to follow a desired momentum trajectory. This 
allows for precise control of the position and velocity of the mass center for a 
system of rigid bodies in open-chain or closed-chain configurations. 

— Allow for smooth transitions between open-chain and closed- chain configura- 
tions. When a system undergoes abrupt changes in kinematic (holonomic) con- 
straints, the number of degrees of freedom abruptly changes. Since the momen- 
tum of the system can be formulated to be independent of the geometry, changes 
in kinematic constraints do not affect the controller. 

- Formulate the rate of convergence to the trajectory as a linear error equation. 
For rigid-body systems, the error equations take the form of decoupled second- 
order systems. The poles of the error equations can be chosen using classical 
design techniques. 

• A control paradigm has been developed to allow switching between different con- 
trol laws during various phases of a task. This is accomplished by splitting up the 
controller into a high-level strategic controller and a low-level dynamic controller. 
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• This control paradigm, implemented experimentally on a free floating space robot, 
has accomplished the following tasks: 

- Pushing off from a structure in a coordinated two arm maneuver. 

— Rotating the vehicle with the momentum wheel while controlling the configura- 
tion of the arms. 

— Landing on another structure. 

Practically, this controller allows the robot to move and orient itself with a minimum 
amount of propellant. Combining system-momentum control with Cartesian-space 
control will allow for more general types of tasks such as crawling along a space 
structure. 

• A free-floating space robot has been constructed. This robot is an autonomous vehicle 
with onboard power, gas thrusters, sensors, and actuators, which floats on an air 
bearing to simulate in two dimensions the drag-free zero-g environment of space. 
Versions of this robot will also be used in follow-on research projects. 

1.4 Reader’s Guide 

This dissertation is divided into eight chapters. The following paragraphs summerize each 
chapter in the thesis. 

Chapter One provides motivation for studying thrusterless robotic locomotion, the 
research goals, and a summary of results. The experiment is briefly described along with 
a list of contributions. 

Chapter Two describes a general design philosophy and overview of the mechanical and 
electrical design. Following the overview is a more detailed discussion of each sensor and 
actuator. Specifications on the various sensors are included when deemed appropriate. 

Chapter Three includes a derivation of the equations of motion for a two-arm free-flying 
robot. This chapter can be skipped upon first reading, while referencing the appropriate 
equations when necessary. It is included here for completeness, and as a reference for 
further research. 

Chapter Four addresses the issues of strategic control, and how higher level task spec- 
ifications are applied to locomotion. This chapter also includes a brief di. ^ussion on the 



Chapter 1. Introduction 


8 


implications of asynchronous switching of dynamic controllers. Chapter Four concludes 
with a description of an implementation of a strategic controller. 

Chapter Five describes the various control laws used in the experiment. Past work in 
computed-torque control is discussed, and a new controller, called system momentum con- 
trol is introduced. Momentum control is derived, followed by a discussion on assumptions 
and limitations for computed torque. 

Chapter Six presents experimental results. A comparison between simulation and ex- 
periment is presented to validate the model and design. Finally, experimental and Simula- 
tion results are presented for an actual complete leap maneuver. 

Topics in three dimensional space robotic locomotion are addressed in Chapter Seven. 
These include an abstract treatment of orientation in three dimensions, controlling attitude 

in three dimensions, and hardware design issues. 

The final chapter, Chapter Eight, summarizes the results. Conclusions drawn from the 

research are given along with suggestions for future work. 

Appendix A details the calibration procedures, and Appendix B derives a simple model 

for the air bearing. 
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Experimental Hardware 


This chapter describes the experimental hardware 1 and some of the design decisions that 
went into its construction. The experimental robot contains many of the subsystems needed 
for an operational space robot, and faithfully addresses many of the issues in space robotics. 


2.1 Design Philosophy 

The maxim “Things don’t work by accident ” and its corollary “Things don’t work re- 
alistically describe experimental research. All too often, the transition from simulation to 
experimental verification fails to achieve meaningful results because of poor judgement and 
lack of attention to detail. During the design and construction of this robot, the successes 
as weH as the failures were studied, so that improvements could be incorporated into future 

iterations. 

One key design decision, made at the beginning of the project, was to make the robot 

modular, both physically and functionally. Like the layers of a wedding cake, the robot is 

divided into four sections: high pressure layer, actuator layer, analog/power layer, and a 

digital layer. Each layer is inclosed in a cylinder with a base radius of 9.5 in. (241.3 mm). 

The connections between the layers (either gas hoses or electrical wires) are only made 

through the sides of the cylinders, and not through the bases. Adherence to this rule 

insured that no layer would interfere with any other layer, and facilitated the inclusion of 

new capabilities into the design. For example, a year into the design, a 9 inch diameter 

‘Most of the mechanical and electrical" designs were done by Marc Ullman and Ross Koningstein, and 
the author gratefully acknowledges their contributions. 
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Figure 2.1: Experimental Free-Flying Space Robot 

Tie base and arms are capable of planar motion. Tie robot is only 
attached to the environment through a thin fiber-optic cable . 


momentum wheel was added to the actuator layer without major modification to the rest 
of the robot. 

Another important decision was the use of “off-the-shelf” parts which, when possible, 
adhered to publically recognized standards. This rule applied to all parts except machined 
parts and analog electronics, which were designed and built in-house. The reason for this 
rule is simple: if a part conforms to a public standard, then future parts with enhanced 
capabilities could be interchanged with the existing one. Also, a public standard usually 
implies that more than one company is producing the particular item. Thus, the issue of 
sole source procurement was avoided. 
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Figure 2.2: 


Robot Schematic 

Tie , oW , divide i.10 «-* «- rizz: 

tor layer, analog/power layer^ an *\ ohot for floatation, pneumatic ac- 
vesseis provide compressed air , conta j ns batteries, 
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analog electronics and the digttal cotnpote,. Each hnk 




2.2 Mechanical Design 

Ain Bearing and Support System The 

12 ft (1 ' 83 “ X 3 M m) plide a level and smooth surface for the 

ground hat » “ “ ^ The air cushion, approximately 50 pm (0.002 in.) thick, 
“"flow of 0.08-0.1 m’/hr (3.0-3.4 SCPH). The 10 ton tahie is kinemattcally 
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supported on three leveling wedges. Barring seismic disturbances 2 , the table can be level 
to ±42 /trad. Affixed to the sides of the granite table is a bar, consisting of vertical columns 
supporting a 1.5 in. (38.1 mm) diameter horizontal tube. The bar, shown in Figure 2.1, 
is located 6.5 in. (165.1 mm) above the plane of the table so that the robot can grip and 
release the bar using pneumatic actuators. 

The base plate is constructed of 1 in. thick hexel honeycomb sheet bonded between 
1/8 in. (top) and 1/16 in. (bottom) aluminum. This design insures small mass and 
inertia in the base plate while providing structural rigidity. The bottom plate was lapped 
flat to 25.4 /zm. The air supporting the bearing comes primarily from a center hole, 
with six additional holes located 208.2 mm from the center. The six secondary holes are 
constricted with small tubes, and act to provide additional stability and prevent grounding 
to the bearing when the center of mass of the entire robot is not located directly over the 
geometric center of the air bearing. Rehsteiner [25] has done an extensive theoretical and 
experimental study of air bearings. He has shown that off-center forces tend to ground the 
vehicle or result in accelerating the robot in the direction of maximum flow (the high end 
of the bearing). To minimize these unwanted effects, the robot’s mass center was located 
near the base geometric center. Similar to Alexander’s [1] base plate design, a shallow 
101.6 mm x 0.762 mm plenum was machined under the center hole to provide an initial 
surface area when initiating the gas flow. 

Ideally, one would like a drag-free zero-g air environment in the plane of the table. 
Rehsteiner [25] provides some formulas to calculate viscous drag and gas flow. For a round 
flat-bottomed base with outer radius r Q , plenum radius r, and center of mass located at 
the geometrical center, the drag force D is: 

D = 

!fl( r 2 - r?) = 0.0612 [ N-s/m ] (2-1) 

h 

where n is the coefficient of viscosity of the gas (air), h is the thickness of the air bearing 
(50.8 /zm nominal) and A v B ’ is the velocity of the center of the base plate with respect to the 
inertial frame. With a mass of 75 kg, this corresponds to a time constant of approximately 

2 During the Loma Prieta earthquake measuring 7.1 on the Richter scale, the 10 ton granite table was 
shaken off its primary balance and onto a secondary support structure. The table also managed to embed 
itself about three inches into a wall. Fortunately, no damage was done to the table or the robot. 
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20 minutes. The flow rate Q is given by: 


Q = 


mgh 3 

M'l - W) 


( 2 . 2 ) 


Equation 2.2 clearly shows that the flow rate needed to float the robot increases with the 
cube of the gap thickness and decreases linearly with the area of the base. The derivation 
of this equation is given in Appendix B. 


Gas and Pressure System Figure 2.3 schematically shows the layout of the pressure 
system [4]. Air is stored on-board in three spherical pressure vessels (maximum working 
pressure of 3500 psi). The tanks axe filled via a flexible hose from a gas cylinder. A primary 
regulator supplies pressures up to 150 psi to the thrusters, while a second regulator provides 
pressure of 9 psi to the grippers. Air flow to the bearing is controlled through a Ametek 
Type 7010 flow meter. 

The on-board gas capacity is sufficient to float the robot for over six hours, assuming 
an initial pressure of 1000 psi (6.98MPa). However, if the gas thrusters are used, the gas 
system can sustain the air bearing for only 20 minutes before running out of compressed air. 
This limitation in the laboratory experiment will also affect the operational capabilities of 
a space robot. For this reason, it is important to study methods which minimize thruster 
usage. 


2.3 Electrical Design 

Unlike fixed base robots, which derive their electricity from the power company, mobile 
robots must store and condition their own electrical power. This fact severly constrains the 
usefulness of any mobile robot by limiting time of operation and amount of peak actuator 
authority. The current design for the free-flying robot allows for about 20 minutes of 
operation before draining the batteries. On board the robot are two battery packs, each 
containing two NiCd rechargeable batteries to give raw power at ± 12 V. The batteries 
are rated at 7 A-h, and can provide 15 A peak current. The raw power is conditioned by 
onboard DC-DC power converters, supplying regulated voltages of +5 V @ 10 A, ±12 V @ 
2.5 A, and ±15 V @ 2.0 A. When the robot is not in use, and external power supply (15 V 
@ 15 A) recharges the batteries and supplies electrical power to the robot. The external 
power cable is removed before operation and experimentation. Of course the external power 
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Figure 2.3: Gas Subsystem Schematic 

Schematic diagram of satellite robot onboard gas subsystem. High pres- 
sure air is stored in three pressure vessels. The pressure is regulated to 
100 psi for use by gas thrusters. A second regulator reduces the pres- 
sure to 10 psi to pneumatically actuate the grippers and supply air for 
floatation. 


cable could be used in place of the batteries, but this limits the range of mobility and the 
achievable accuracy. Also, when multiple robots cooperate together, the issues of tether 
fouling will limit their usefulness. 

Table 2.1 describes the different analog cards used for switching power, filtering sensor 
signals, and driving motors. 
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Number Component Comments 

1 Power Control Unit Main power switch and fuses 

2 Battery Charging Boards Charge batteries when connected to external power 

2 Safety Board Drives solenoids and shuts off raw power to bus on error 

3 Motor Driver Boards Each boards drives 2 DC motors 

1 RVDT Board 4 rate and position channels per board 

1 INS Sensor Board Filters for accelerometers and rate sensor . — 

Table 2.1: Analog Cards 


2.4 Sensors 

Joint- Angle Sensors Each arm joint is instrumented with a Rotary Variable-Differential 
Transformer (RVDT) to provide angle measurements. After calibration and filtering, the 
angles are estimated to an accuracy of ± 2 mrad. The RVDT angle signal is low-pass 
filtered at 353 Hz. No anti-aliasing filter was employed because the high frequency content 
of the signal was not sufficient to justify the added phase delay. 

An estimate of the angular rate is obtained by passing the position signal through a 
pseudo-differentiation filter. The transfer function of the filter can be realized as 
where l/2xa = 12 Hz , 1/2x6 = 330 Hz and k = 0.968. At low angular rates (s < 0.5 Hz), 
the filter approximates a pure differentiator. One reason an RVDT sensor was used instead 
of an encoder was the ease in obtaining reasonable rate information at low frequencies. 
During normal operation, the maximum angular rate of any joint does not exceed 0.1 Hz. 
The rate information is accurate to ± 2 mrad/s. 

Angular Rate Sensor Mounted on the base plate in an isolation shock mount is the 
Watson Angular Rate Sensor. This sensor measures the angular velocity of the base plate 
with respect to the inertial frame (laboratory frame). The sensor consists of two pairs of 
piezoelectric bender elements, mounted end to end, but rotated 90 degrees as shown in 
Figure 2.4. The base element is driven at 360 Hz causing it to vibrate. If the instrument 
is rotated, Coriolis forces cause momentum to be transferred into the perpendicular plane, 

resulting in a bending of the sense-element. 

After calibration, the instrument can detect angular rates of ±1 mrad/s, with average 
bias uncertainty of ±60 /irad/s over a 5 minute time interval. The maximum angular rate 
the sensor can detect is 100 deg/s (1.74 rad/s). Typical peak angular rates are 0.6 rad/s 
for the base body. Table 2.2 gives some important specifications for the instrument. 
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Figure 2.4: Angular Rate Sensor 

Schematic diagram of the angular rate sensor as a “ Tuning Fork” trans- 
ducer. 

Unfortunately, angular rates are not the only source of sense element bendings. External 
vibrations and accelerations can deflect the sense element, as can acoustical energy which 
is radiated from the drive element and reflected by its environment to the sense element. 
In addition, temperature variations causes drift in the DC component of the signal. By 
using a “tuning fork” configuration, many of these problems are reduced. By driving 
the two pairs of bending elements at the same frequency but 180 degrees out of phase, 
a nodal plane created midway between the drive elements cancels radiated energy from 
the drive elements. Also, external vibration or acceleration cause the sense elements to 
vibrate in phase, while angular rates cause the sense elements to vibrate out of phase. 
Thus, differencing the sense element signals achieves common-mode rejection of external 
vibration and accelerations. 

The only other major cause of error is acoustical energy which is radiated from the 
drive elements, reflected by the environment, and interpreted by the instrument as a rate 
signal. An electrical analogy is energy being reflected from the end of a transmission line 
because the impedance at the end is not matched with the terminating resistors. This 
problem was solved by wrapping the instrument in 1/8 inch Sorbothane, a visco-elastic 
polymer. The Sorbothane provides acoustical damping and shock absorption bv absorbing 
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Model: 

ARS-C131-1A 

Power supply: 

±15 VDC ±5% 20 mA maximum 

Output: 

± 10 VDC at full scale angular rate 

Sensitivity: 

± 100 deg/s full scale 

Output current: 

± 10 mA maximum 

System frequency: 

360 Hz nominal 

Scale factor error: 

2% 

Linearity: 

< 0.1% full scale 

Frequency response: 

DC to 30 Hz 

Output noise: 

5 mV RMS maximum 

Life: 

50,000 hours MTBF minimum 

Shock: 

200 g 

Weight: 

0.110 kg 


Table 2.2: Watson Angular Rate Sensor Specifications 


or transmitting most of the energy. 

The angular rate sensor, also known as a “tuning fork gyro”, has the advantage over 
conventional rate gyroscopes of longer life (50,000 hours MTBF) and reduced cost. 


Accelerometers The robot is equiped with two Systran Donner 4310 accelerometers. 
Table 2.3 lists the major attributes of the instrument. Historically, accelerometers have not 
been used in robotics because of low signal- to- noise ratio in the presence of gravity. It was 
hoped that the air bearing would afford a stable environment so that accelerations down to 
the 100 ng level could be measured to give reasonable velocity and position measurements. 
Unfortunately, drift in the instrument and environmental disturbances did not allow for 
centimeter accuracy in position over a one minute time scale. However, a scheme that shows 
good promise in the future is to use a Kal m a n filter with position measurements from off- 
board vision combined with accelerometer data to give smooth estimates of position and 
velocity. 

LED Sensor/detector The LED’s are used to detect objects which come into close 
proximity to the grippers. They are driven by very short pulses of current (100 mA) at 
a nominal period of 1.4 kHz. During one duty cycle, the LED is on for 12 ps and off 
for 700 ps. The detector, a phototransistor, senses the short pulse and extends them in 
order to provide a constant DC level. Pulse extension is accomplished by a retriggerable 
monostable multivibrator (74LS122), with a period of 2 ms. Therefore, any break in the 
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Model: 

Systran Donner 4310 

Power supply: 

±15 VDC ±10% at 10 mA maximum 

Output: 

±7.5 VDC at full range 

Sensitivity: 

±1 g full scale 

Output current: 

±3 mA maximum 

Zero Output (Null): 

< 0.05% full range 

Linearity: 

< 0.05% full scale 

Natural frequency: 

50 - 250 Hz 

Output noise: 

< 7.5 mV RMS 

Resolution 

< 0.001% full range 

Shock: 

lOOg for 11 ms 

Weight: 

128 grams 


Table 2.3: Linear Servo Accelerometer 

light pulses of longer than 2 ms will cause the output of the detector to drop from 5 volts 
to 0 volts. 


Force Sensors The robot is equiped with three force sensors embedded into each gripper. 
Two sensors are located in the palm, and the other sensor is located in the pneumatically 
actuated finger. When calibrated, the sensor can detect forces between 0 N and 9 N to an 
accuracy of 1% of full scale. 

Recent advances in micro machining and surface mounted devices (SMD) technology 
has given rise to solid-state force sensors, pressure sensors, and accelerometers. For exam- 
ple, a small force sensor (3 mm x 3 mm) can be produced by micro machining a thin shelf 
in the silicon and growing a half bridge circuit on it. Changes in applied force produce 
linear changes in resistance. The advantage this method has over strain gages is that the 
force sensor is located at the point of interest, and not some distance away. Strain gages axe 
usually attached to a beam element, and infer the force at the tip by measuring the strain 
or bending in the beam element. To achieve greater sensitivity, a longer beam is necessary 
which introduces higher order dynamics. It is also more difficult to measure applied forces 
on a smooth surface using an array of strain gages. These problems can be overcome by 
using an array of solid-state force sensors. 
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2.5 Actuators 

There are two basic types of actuators on the robot: electric DC motors and pneumatically 
actuated solenoids. The five motors supply torque to the four joints in the arms and to the 
momentum wheel. Eight thrusters, mounted in 4 orthogonal pairs on a flat lexan thruster 
plate can also supply additional momentum to the vehicle. During normal operation (no 
thrusting), the air supply can float the robot for about six hours if the tanks are initially 
pressurized to 1000 psi (6.98 MPa). If the thrusters axe used, however, the vehicle can only 
float for 15 to 20 minutes before grounding out. 



Figure 2.5: Arm Schematic 

Each link is driven by a limited-angle torque motor; the arm is equipped 
with joint angle and optical endpoint sensors. 


Motors Each link is driven by a limited-angle DC torque motor. These motors were 
chosen for their nearly frictionless operations. Each arm has two motors, one for the 
shoulder and one for the elbow. Figure 2.5 shows a schematic of the arms and motors. The 
shoulder joint is direct drive, while the the elbow joint is driven through a cable and pulley 
system. The pulley is geared 4:5 to extend the range of the elbow joint at the expense of 
reduced torque capability. This configuration allows the elbow motors to be mounted on 
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the shoulder, and drastically reduces the effective inertia of the upper-arm link. The elbow 
motor is carried by a yoke attached to the shoulder-motor shaft to allow it to operate m 
its full range independently of the shoulder position. 

The robot is also equipped with a DC brush servo motor to drive the momentum wheel. 
This motor was chosen so as to supply enough torque to rotate the robot 180 deg in 10 s 
while the robot is in a nominal position 3 . Due to onboard power constraints, only three 
of the five motors can be run at maximum torque at the same time. 


Base Parameters 

Diameter 

0.4826 m 

Mass 

67.96 kg 

Inertia 

3.29 kg-m 2 

Shoulder Link Parameters 

Length 

0.3048 m 

Mass 

1.9231 kg 

Inertia 

0.0238 kg-m 2 

Hub to Link Center of Mass® 

59.4 mm 

Motor Peak Torque 

0.91 N-m 

Elbow Link Parameters | 

Length 

0.3015 m 

Mass 

0.3382 kg 

Inertia 

0.00416 kg-m 2 

Hub to Center of Mass 

0.1058 m 

Motor Peak Torque 

0.59 N-m 

Momentum Wheel Parameters 

Mass 

3.2943 kg 

Inertia 

0.1025 kg-m 2 

Motor Peak Torque 

0.54 N-m 


“The center of mass is offset by 3.7 mm from the link 
centerline. 


Table 2.4: Arm and Base Specifications Table 


Grippers Each arm is equipped with a special gripper to allow the robot to detect and 
grasp a bar surrounding the granite table. The reason for the left and right handedness 4 
in the design, apart from preserving symmetry, was to allow a wider range of grips. Each 
hand has one pneumatically actuated finger and solid-state force sensors located in the 


3 The inertia of the robot taken about the system mass center is a function of configuration. 
4 Each gripper is the mirror image of the other. 
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palm and finger. There is also a photo emitter/ detector pair to sense when an object is 
within its grasp. The center of mass of each hand is located at the wrist, which is free 
to pivot along an axis perpendicular to the table. The palm and finger are padded with 
Sorbothane to supply compliance and prevent bouncing. Although there is no motor in 
the wrist, the air tubes and wires running through the hand supply a small restoring force 
to bring the hands to a nominal position. This force was considered small and was not 
modelled. Figure 2.6 shows a picture of the gripper. ORIGINAL PAGE 



Figure 2.6: Gripper 

Close up picture of the gripper. Each hand is instrumented with three 
forces sensors and an optical sensor to sense the bar. The finger is pneu- 
matically actuated . 


2.6 Computer System 

The computer system incorporates the UNIX development environment with high-performance 
real-time hardware located onboard the robot. A heterogeneous network of Sun Worksta- 
tions, running the UNIX operating system, allows a window-based environment for pro- 
gramming, debugging, analysis and simulation. The real-time computer communicates 
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Number 

Component 

Manufacturer & Model 

Comments 

1 

Processor 

Motorola MVME- 147 

68030 / 68882 with 4M RAM 

1 

Analog Input 

Xycom XVME-590/3 

16 Channel Analog Input 

1 

Analog Output 

Acromag AVME-9210 

8 Channel Analog Output 

1 

Digital I/O 

Xycom XVME-290 

32 Channel Digital I/O with Timer 

1 

Ethernet Transceiver 

Versitron LE-220 

Fiber Optic Ethernet Transceiver 


Table 2.5: Real-Time Computer Components 


with the network via a fiber-optic ethernet cable (see Figure 2.2), to allow programs to 
be downloaded and experimental data uploaded to the server’s hard disk. The computer 



Figure 2.7: Hardware Architecture 

The computer system combines the Sun UNIX development environment 
with a high-performance real-time computer . 


layout is shown schematically in Figure 2.7. All Sun Workstations in the lab are connected 
via ethernet and gateways to a local file server. The real-time computer is also connected 
to the network and looks to the other machines as another diskless workstation. The main 
CPU card, a Motorola MVME 147 board consists of 4M RAM, a 68030/68882 processor, 
4 serial ports and an ethernet port. All the analog electronics interface to the digital elec- 
tronics through a digital transition module, which connects sensor signals to the digital 
board’s P2 connectors. Table 2.5 summarizes the real-time system hardware. 
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Derivations of the Equations of 
Motion for LEAP Vehicle 


3.1 Introduction 

This chapter describes a general method for deriving the equations of motion for a free- 
flying robot. Although alternate formulations have been done for one-arm [1] and two- 
arm [5] robots, this derivation will focus upon extracting momentum equations. These 
equations, presented at the end of this chapter as Equation 3.29, will be used m Chapter 5 

in conjunction with computed-torque control. 

Figure 3.1 shows a schematic of a generic two-arm robot with a momentum wheel. 
In the free-flying configuration, the robot has 8 DOF. Of course other configurations are 
possible, namely, whenever one or two arms grasp another structure. When both arms 
grab a bar, the system is said to be in a closed-chain configuration. In the closed-chain 
configuration, the number of degrees of freedom drop to 4, and the equations of motion 
can be made to look very different. By formulating the equations of motion in terms of 
momentum as in Equation 3.29, only one set of equations is needed, because momentum 
is a dynamic quantity that is independent of the geometry. The kinematic constraint will 
be accounted for through the nonholonomic constraint equations of section 3.3.1 
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3.2 Definitions of the Generalized Speeds 

Kane’s method [15] will be used for deriving the equations of motion for the LEAP system. 
Both the free-flying and closed-kinematic chain configuration (arms grabbing the bar) will 
be derived. This process involves choosing a set of generalized coordinates and general- 
ized speeds, finding the partial velocities, and then deriving the dynamical equations of 
motion. Having done this, the closed-kinematic-chain derivation is a special case of the 
open-chain derivation. As a cross-check to the derivation, a computer generated version of 
the equations of motion were also performed using the software program SD/EXACT [31]. 
This program produces as its output a FORTRAN subroutine containing the equations 
of motion. The results of the SD/EXACT program were simplified using MACSYMA [2], 
a symbolic manipulation package. At the present time, SD/EXACT does not offer the 
flexibility in choosing generalized velocities and coordinates, which can give equivalent but 
simpler equations of motion. For this reason, most of the analysis was performed by hand. 

Although not unique, the choice of generalized coordinates is predicated on ease of 
computation. The generalized coordinates represent quantities that are easily measured, 
as shown in Figure 3.1. The only issue that remains after choosing a set of generalized 
coordinates q, is to determine the zero or reference configuration. Three criteria were used 
to pick the unique reference configuration. 

1. The “x” direction of the base should point towards the front of the vehicle to conform 
with conventional navigation nomenclature. In deriving equations of motions for an 
airplane, the “x” direction points in the forward direction. 

2. The “x” direction of each link should point along the length of the arm. In beam 
theory, the body coordinates are defined such that the tt x” direction is along the 
beam and the w y” direction is along the direction of deflection. 

3. In the reference configuration when all the q,’s are zero, all the body coordinates 
should line up. For example, b\ and f x would be parallel. 

Note that all subscripts begin at zero instead of one (i.e. the first element of the vector 
u is u Q ). This corresponds to the definition of vector elements in the C language, where 
the first element of the array x is x[0]. Also, the terms sin ($) and cos (g.) are abbreviated 

as Si and c,. 

The choice of generalized speeds is also not unique: however, they must be affine in the 
derivatives of the generalized coordinates. In the Lagrangian formulation of the equations 
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Figure 3.1: Dynamic Model of the Mobile Space Robot 

Model description is in the text . 
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of motion, the identity map relates the derivatives of the generalized coordinates to the 
generalized speeds. This apparent simplification can lead to complicated expressions in the 
dynamical equations of motion. A choice of generalized speeds which has been found to 
give simple expressions for the equations-of-motion are: 

U 0 = A v B ' • fei = qo cos ( 92 ) + 9i sin(<fe) 

ui = A v B ‘ ■ b 2 = qi cos (q 2 ) - q 0 sin (g 2 ) 

A a B i_ 

1*2 = W *03 = 92 

1*3 = A U> C * bz = 92 + 93 

1*4 = * 63 = 92 + ?3 + ?4 

1*5 = ^ * 63 = 92 + 95 

1*6 = A Ll> F * 63 = 92 + 95 + 96 

u 7 = * 63 = 92 + 97 

where the velocity of point B* in reference frame A is noted by A v B * . Using these definitions 
for the generalized speeds, the velocities of the points of interest become: 

A v B * = u 0 bi + uib 2 

A V Pz = Uq 61 + 1*1 62 + 1*2 £ 0 1 7*2 

A vP * = l * 0 61 + l*i 62 + u 2 Lqi T 2 + 1*3 ill, c 2 — 1*3 illy C 1 

= Uq 61 + t*i 62 + 1*2 Xqi 7*2 + 1*3 ill c 2 

^1?^* = Uq 61 + 1*1 b 2 + 1*2 £oi 7*2 + 1*3 ill c 2 + U A il2 t &2 ~ 1*4 il2 v ^1 

A V Pb = tio hi + 1*1 62 + 1*2 ioi 7*2 + u 3 ill c 2 + 1*4 il2 ^2 (3*2) 

= 1*0 hi + 1*1 &2 + 1*2 i 02 7*4 

A V E * = l*o hi + t*i h 2 + 1*2 L 02 7 * 4 + 1*5 i21« c 2 "" u 5 B 2 \ y e\ 

A V Pt = 1*0 hi 1*1 b 2 + 1*2 £02 7* 4 + 1*5 £21 €2 

A v p * = 1*0 hi + 1*1 b 2 + t *2 £02 7*4 + 1*5 i 21 c 2 + 1*6 L 22x f 2 1*6 £ 22 y / 1 


= 1*0 hi + 1*1 b 2 + 1*2 £02 7*4 + 1*5 £21 e 2 + 1*6 £22 f 2 

= Uo hi + l*i h2 + 1*2 £03 7* 6 


(3.3) 
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3.2.1 Accelerations 

By differentiating the above velocities with respect to time in the inertial frame A, the 
following eight accelerations of interest are found: 


A a B * 


A a c * 


A a D* 


A a E ‘ 


A a F * 


A a G ‘ 

A a Pi 


A a p * 


UO hi + tii 62 — t*lt*2 hi + Uo u 2 &2 

iio hi + «i h 2 — U1U2 hi + uo «2 f >2 + U2L01 T 2 + usLii; c 2 — 

U3-hll v C 1 — ti2 2 -hoi r l — u 3 2 Lll x Cl — U 3 2 Lu t Cj 

Uq 61 + Ui 62 — Uiti2 hi + Uo«2 &2 + «2^01 r 2 + W3I/11 C2 + 

U4-Ll2 x <^2 ~ ^4-I / 12 ll <fl — ti 2 2 ioi 7*1 — u 3 2 I'll C 1 — U 4 2 -£'12 I <^1 — u 4 L\2 y d 2 

ilo hi + til 62 — til 1*2 fei + U0U2 h2 + U2L02 7*4 + “5-^21, 62 — 

ti5-h21 y «1 — U 2 2 Lq 2 7*3 — U5 2 Z-21, e l — «5 2 -h21„ c 2 

«o hi + iii 62 — U1U2 hi + U0U2 h2 + U2L02 t* 4 + “5X21 ®2 + 

UeL22 x f 2 - ti6-h22y /1 - U2 2 -ho2 7*3 “ tis^l «1 ~ ^22 x f 1 ~ u 6 2 ^22,, A 

ilo hi + til 62 - u lti2 hi + U0U2 h2 + U2X03 7*6 — ti2 2 -ho3 7*5 (3-4) 

Uo hi + til &2 — U1U2 hi + UqU2 h2 + U2Z.OI 7*2 + «3-hll c 2 + 

U 4 L 12 d 2 — ti2 2 Xoi 7*1 — U 3 2 Ln Cl — «4 2 Z/12 <il 

tio hi + tii 62 — t*i 7*2 hi + ti()ti2 &2 + U 2 L 02 7*4 + U5.L2I e 2 + 

‘U-&I J 22 f 2 — ti2 2 -ho2 7*3 — U5 2 Z2I ®1 — u 6 2 ^22 /l 
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3.2.2 Partial Velocities and Partial Angular Velocities 


The next step in the derivation of the equations of motion is to calculate the partial 
velocities A vf i and partial angular velocities A uf where 


a„p. * 


A ,.,B * 


du T 

du T 


(3.5) 

(3.6) 


The tables of the partial velocities of the points of interest and the partial angular velocities 
of the bodies of interest for the robot are given on the following page. 


3.3 Equations of Motion 

The equations of motion can now be derived using Kane’s dynamical equations [15] which 
state that: 

F r + F r " = 0 r = 1 ...7i 

where n is the number of generalized coordinates or degrees of freedom, and 

f; = E -m * Av ? ' ai 
*= 0 

F t = E V* • * 

i=0 

v is the total number of particles and Ri is the resultant of all forces acting on particle P t . 
By combining terms, one can put the equations of motion into the form 

M U - C = Tr + / external 

q = Y' l u 

where M is a symmetric positive definite mass matrix, c is an 8 x 1 vector of Coriolis 
and centrifugal terms, r is a 5 X 1 vector of applied motor torques and / external IS an 
8x1 vector of all other external unmodelled active forces acting on the system. We will 
assume that f extcr nai = 0 in the following derivation. The unimodular matrix Y~ l defines 
the kinematic relationship between q and u. 
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0 
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0 
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Where the z, are constants and have the following definitions: 

Z\ = mo + m.! + TTl2 + m 3 + m 4 + m 5 

z 2 = —(mi + m2)Xoi y — (^3 + m A )L 0 2 y ~ msLoZy 

z 3 = -rniln, - m 2 Xn 
Z4 = —TJl\L\\ y 
z 5 = 

z 6 = -m 2 Xi2 v 
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*7 

A 

—7713X21, — 7714X21 

*8 

A 

-7713X21,, 

*9 

A 

-7714X22, 

*10 

A 

-77x4X22,, 

*11 

A 

(tTXi + 77X2 )X 0 1, + ( 77X3 + 77X 4 )X 0 2, + ^5^03, 

*12 

A 

J 0 + (TTXi + 77X2 )Xo1 2 + (77X3 + 77X 4 )Xo 2 2 + 77X 5 Xo3 

*13 

A 

-77XiXoi, Xll y + 77li XoiyXn, + 77X2X01^X11 

*14 

A 

77XiX 0 l,Xn J + 77XiXoi v Xii # + 77X2X 0 1 ,Xii 

*15 

A 

— 77l2Xoi, Xi2 y + 77l2Xoi y Xl2, 

*16 

A 

77X2^01,^12, + m 2J’01 y Xi2 y 

*17 

A 

-Tn 3 Lo 2 x L 2 l y + 77X3X02^X21, + 77X4Xo2 y X21 

*18 

A 

77l3Xo2, X2I, + 77X 3 Xo2 y X21 y + 77X4 Xo2,X21 

*19 

A 

— 77X4X02, X22,, + 77X4 Xo2 y X22, 

*20 

A 

77X4 X02, X22, + 77X4Xo2„X22 v 

*21 

A 

Jl + 77Xi(Xn, 2 + Xn„ 2 ) + 77l2Xn 2 

*22 

A 

— T7l2XiiXi2 y 

*23 

A 

77X2X11X12, 

*24 

A 

J2 + 77X2(Xi2, 2 + Xi2„ 2 ) 

*25 

A 

J3 + 77X 3 (X21, 2 + X 2 l y 2 ) + 7714X21 2 

*26 

A 

— 7714X21X22,, 

*27 

A 

77x4X21X22, 

*28 

A 

J4 + 77X4(X22, 2 + J<22 v 2 ) 

*29 

A 

h 


The choice of generalized speeds determines the complexity of the mass matrix. With 
the present choice, the mass matrix can be formulated with 28 multiplies and 14 additions. 
These operations can be performed in parallel, since there this no dependency between the 
elements of the mass matrix. Also note that the quantities q u ?2, and q 3 do not appear in 
the mass matrix or the nonlinear Coriolis and centrifugal terms. 

The nonlinear Coriolis and centrifugal terms are found in the vector c for Equation 3.7, 

and are functions of qi, u, and Z{. 
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c(0) = Ui tl 2 Zl + U 2 2 Zu + U 3 2 (z 4 S 3 “ *3 C 3) + «4 2 (^ s 34 ~ *5 C 34) + 

«5 2 (28^5 “ Wi) + ti6 2 (*K)S56 “ z 9 c h&) 
c(l) = -U 0 ti 2 *l - «2 2 22 - « 3 2 ( 23^3 + *4 C 3) ~ «4 2 (*5 5 34 + Z 6 C 34 ) ~ 

W5 2 (^75s + Z8 C 5) ~ u 6 2 ( 2 9 a 56 + ^10 c 56) 
c(2) = -U 0 U 2 Zll + UlU 2^2 + «3 2 (Z14«3 - Z 13 C 3 ) + «4 2 ( z 16^34 “ ^15^34) + 

u 5 2 ( z 18 5 5 ~ z 17 c s ) + u 6 2 ( 2r 20 s 56 — 219C56) 
c(3) = -Uolt 2 (z4S3 - Z3C3) + UiU2(z3S3 + Z4C3) - U2 2 (zi 4 S 3 - Zi 3 C 3 ) + 

U4 2 (Z23S 4 ~ * 22 C4) 

C(4) = -U 0 U 2 (Z6^34 ~ 25C34) + UlU 2 (z 5 S34 + 2 ^ 34 ) - U 2 2 (ZiqS 3 4 “ ^15^34) ~ 

U 3 2 (z 23 S4 - Z 22 C 4 ) 

c( 5) = — 12otl 2 (^8^5 ~ z 7Cs) + Ul u 2( 2: 755 + 2fcCs) ~ u 2 2 ( 2: 18 s 5 — z 17 c s) + 

u& 2 (z 27 so - z 26 c e ) 

c(6) = -U 0 U 2 (zio556 - 29 C 56 ) + «lU2(29«56 + Z 10 C 56 ) ~ «2 2 (220«56 “ 2i9C 56 ) ~ 

U5 2 (227S6 ~ ^26^6 ) 

c (7) = 0 

The vector Tr in Equation 3.7 is given by 


Tt = 


0 0 0 0 0 

0 0 0 0 0 

-1 0-1 0 -1 

1-1000 
0 10 0 0 

001-10 
0 0 0 1 0 

0 0 0 0 1 


ro 

T\ 

T2 

T3 

T4 


(3.9) 


Where th^ r t are torques from the five motors. Note that the sum of each element in a 
col umn of T is zero. The physical implications of this fact is that no motor can cause a 
net torque about the system’s mass center. The first two rows are zero, signifying that the 
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actuators have no net effect on the total linear momentum. The Y 1 matrix in Equation 3.7 
is unimodular and is given by: 


cos(g 2 ) 

- sin ( g 2 ) 

0 

0 

0 

0 

0 

0 

sin (<&) 

cos(q 2 ) 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

-1 

1 

0 

0 

0 

0 

0 

0 

0 

-1 

1 

0 

0 

0 

0 

0 

-1 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

-1 

1 

0 

0 

0 

-1 

0 

0 

0 

0 




3.3.1 Nonholonomic Constraint Equations 

When both arms of the robot are grasping the bar, the number of degrees of freedom of the 
system drop from eight to four. At the tip of each arm, there two nonholonomic or velocity 
constraints which much be satisfied to insure the closed kinematic-chain configuration. The 
four constraint equations are: 

A v p > ■ bi = 0 ( 3 - 10 ) 

A v p > • b 2 = o (3- 11 ) 

A v p * . bi = 0 (3-12) 

A v Pi -&2 = 0 (3.13) 

Substituting Equation 3.2 and Equation 3.3 yields the following set of nonholonomic con- 
straint equations. 

(3.14) 




U s = 

A„ u T 



COS (33+94) 

sin(Q3+94) 

— Loi x »in(93+94)+i , 0l« cos(93+?4) 



Ln sin(94) 

Ln »in(<74) 

Lu sin(94) 


Uz 


cos(< 73 ) 

sin( 93 ) 

Loi* *in(^3)“Loitt co*(<73) 


Uo 

u 4 


L 12 8m(<74) 

Ln 

Lu *in(g4) 


lil 

Us 

- Ue - 


COS ((75+96) 
L 21 sin(<76) 

»in(9j+7e) 
L 2 1 sin(9e) 

-Lo 2 x »in(g5+96)+Lo2^ cos(q 5 +96) 
L 21 sm(gs) 


u 2 m 


COs(<?5) 

sm(9s) 

Lq 2 x sin(95)-Lo2« cos(qs) 



L L 22 ain(< 7 sy 

La 2 

La 2 »in(g 6 ) 

J 
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Differentiating the velocities in Equation 3.2 and Equation 3.3 with respect to time in 
reference frame A and dotting the acceleration vectors with orthogonal unit vectors 6i and 
&2 gives the following set of equations which expresses u$ in terms oi it r ,u and q . 

u B = AstUt + b $ (3.15) 



cos(q3+q 4 ) 

ain(? 3 + 94 ) 

-Loi, sin(g 3 +g 4 )+£oi v co »(< 73 -h 94 ) 



L\\ sm(g 4 ) 

L11 *111(94) 

Lu *in( 94 ) 


*3 


COs( 93 ) 

sin(g 3 ) 

Loi x sin(g 3 )-Loiv 003(93) 


uo 

. 


Li 2 sin(g 4 y 

L12 sin(g 4 j 

Li 2 sin( 94 ) 








. 


= 





til 



cos(g 5 + 96 ) 

sinfgs+gc) 

-L02* *in( 95 +g 6 )+^ 02 v cos(g5+96 ) 


ti? 

_ tie _ 


L21 8in(g«) 

L21 ain(g6) 

L21 sin( 96 ) 


*"4 


cos (95) 

sm(g 5 ) 

Lq 2 x sin( 95 )-^ 02 y 003(95) 



L L22 *in(gey 

L2 2 * in(g«) 

L22 sin (96 ) 



r -UQU 2 sinfo+gQ+tt!^ co»(g 3 +q4)+i*2 a [&0i» cos(93+94)+£q1i; »m(93 +?4 ))+“3 2 ^11 cos((?4)4-U4 2 Li2 

Ln*m{qi) 




+ 


uqu; sin(g 3 )— uiU2 co8(g 3 )— t^tLoig sin(g 3 )]— u 3 2 Iqi ~" u 4 2 £i2 cos(g 4 ) 

— “ ** L12 am(qi) 

-uqu 2 nn(qs+q4)+uiu 2 cos{q 5 +qe)+U2 2 [Lo2 x cos(q!,+q*)+Lo2 v am^s+qs)]*^ 2 ^! cos(q G )+UG 2 L 2 2 

L 2 i sin(gej 


UQtt 2 sin(gs) — COsfcs) — U2 2 (£p2g co8 (qs)+£o2 v Sinfcs)]— u 6 2 ^22 CO»(96) 

_ L22*in(9e) 

With this constraint equation, one can derive the reduced set of the equations of motion 
by partitioning Equation 3.7 and adjoining the constraint equation as follows 


M rr M r 
M T ra M s 


-1 

- 


■ 

r 


U r 

= 

Cr 

+ 


Us 


c. 



T r 

T, 


^iWV r + Af ra A aT "f" (A/r t-^-ar'}^' "t" AgfA^^A ^ j 
[c r + Ale, - M ra b a - AlM„b a \+ [t t + AlT a ] T 
This can be written in a more succinct form: 


(3.16) 


(3.17) 


Mu r - c = Tt 


(3.18) 


X2 
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3.4 Force Constraint 

There is an alternate but equivalent formulation of the equations of motions which does 
not involve solving for the nonholonomic constraints, but rather imposes a force boundary 
condition at the tips to insure that the velocity at the tips of the arms is zero when the 
arms are grasping the bar. If Equation 3.7 is modified to include forces at the tip we get 


Af tl C — Tr "t~ 7/ tip 4" f external 


(3.19) 


Where f tip is a four vector representing the normal and tangential forces exerted by the 
bar on the arms at the tips, points P5 and P&. 


T = 


COS (93 + q 4 ) 

■ sin ( g 3 + 94 ) 


sin (93 + 94) 

- cos(g 3 + 94) 


- cos(? 5 + q 6 ) 

- sin(?5 + q 6 ) 


sin (95 + 9e) 
- cos(9 5 + 96 ) 


— ioi x S34 + Z/01„ c 34 — ■£'01 i C 34 — 7oi y S34 — ■f'02* s 56 + X>02 y c 56 702, c 56 7o2 v S56 


-in sin ( 94 ) 
0 
0 
0 
0 


-in cos( 9 4 ) 
-i 12 
0 
0 
0 


0 

0 

-i 2 i sin(9e) 
0 
0 


0 

0 

— i 2 l COS(96) 

— i 2 2 
0 


Rewriting Equation 3.14, yields 


Au = 0 


(3.20) 


1 0 — ioi v 
1 Loi, 
0 -io2 v 
L 02 , 


0 

1 

0 1 


— illS3 — il2 s 34 

illC3 T12C34 
0 
0 


0 0 

0 0 

0 —L 21 S 5 — i22 a 56 

0 i21 c 5 i22 c 56 


U 0 
Ui 

u 2 

«3 

U 4 

U5 

U6 


= 0 


Differentiating Equation 3.20 with respect to time in reference frame A yields 


Aii = b 


(3.21) 
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Ul u 2 + u 2 2 L 0 i s + u 3 2 In cos(g 3 ) + u 4 2 Li 2 cos (q 3 + q 4 ) 

-u 0 u 2 + U 2 2 Ioi„ + u 3 2 L n s\n{q 3 ) + u 4 2 Lu sin (93 + < 74 ) 

U\U2 + U 2 2 L 02x + U 5 2 L 2 ! cos(? 5 ) + U 6 2 L 22 cos( 9 5 + 9e) 

—uqu 2 + u 2 2 Lo 2v + tt5 2 i 2 i s i n (9s) + U 6 2 L 22 sin(9s + 96) _ 

Using Equation 3.19 and Equation 3.21, the resultant forces exerted by the bar on the 
axms at points P 5 and P$ are 

f tip = [AM~ l T]- l [b - AM~ l c - AM-'Tt] (3.22) 

3.5 Some Properties of the Center of Mass 

In controlling the robot, there are some properties of the center of mass that are of interest. 
They are the position, velocity and acceleration of the center of mass. Also, the total 
angular momentum about the center of mass will be derived. The following definitions will 
be used in calculating the center of mass and momentum for the robot. 

Definition 1 (Mass Center) If S is a set of particles P u ...,P n of masses 
respectively, there exists a unique point S such that 

n 

rniTi = 0 

t=i 

where r, is the position vector from 5* to P, (i = 1, . . ■ , n). The point 5* is called the mass 
center. 

Definition 2 (Linear Momentum) If S is a set of particles P u ..., P n of masses m u . • • 
respectively, moving in a reference frame A with velocities A v Pl ,... A v Pn , then 

L = Y,m, A v p > 

1 = 1 

where the vector L is called the total linear momentum. 

Definition 3 (Angular Momentum) If S is a set of particles P u ...,P n of masses 
m 1 ,...,m n , respectively, moving in a reference frame A with velocities A v p ' , . . ., A v Pn , 
then the vector A H S/S ' , called the angular momentum of S relative to 5* in A ts defined 

as n 

A H s t s ‘ =2>,p, x A v F ‘ 

1=1 

where p, is the position vector from the point S * to Pi (i = 1, • ■ • » n )- 
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The above definitions describe properties of a system of point masses, and do not deal 
with rigid bodies per se. However, for a system of rigid bodies, these definitions readily 
extend by treating each rigid body as behaving like a point mass at its mass center with 
a central inertia dyadic about the mass center. Then definitions can be stated in a more 
useful form as: 


All bodies 

L = jr rm A v: (3-23) 

«=i 

All bodies 

a H s/s ' = £ I* + m,p, X A v* (3.24) 

1 = 1 

where all points are taken to the mass centers of each of the bodies* 

With these definition, the mass center and momentum of the robot can be calculated 
as a function of the state, masses, and lengths. 


,cm _ 


= <7o a i + 9i a 2 


(3.25) 


+ (^11 ” Z3C3 + Z 4 S 3 - Z5C34 + *6^34 “ 27C5 + ^8*5 ~ 29C56 + Z 10 S56)/2i6i 
- (22 + 2 3 S 3 + Z4C3 + Z5S34 + Z6C34 + Z7S5 + Z 8 C 5 + ZgSse + 2ioC56)/2l&2 

= + 9i«2 + |^M(l,i)|6i - |^M(0,t)|b2 

The velocity of the center of mass is given by: 

A V CU = (zitio + Zl u 2 + (Z3 5 3 + *4 C 3) U 3 + 

(Z5S34 + 26C34 )u 4 + (Z7S5 + Z8 c s) u S + (^9 5 56 + Z\O c h&) u &) I z \b\ 

+ (ZjU 1 + ZnU 2 - (Z 3 C 3 ~ ^4«3)«3 - (3.26) 

(Z5C34 - ^34)^4 “ (27C5 “ ^8*5)«5 “ (^56 ~ Z\qSS&)U 6)I Zib 2 

M( 0 , i)u(i)^ 61 + j^M(l,i)u(t)j& 2 j 


Z\ 


The total linear momentum of the system A L CU is ZjC 01 *. 
The acceleration of the center of mass is given by: 


A a cu = — 


1 53 M ( 0 , t)ti(i) - c( 0 )| 61 + 1 2 M ( X ’ 0«(0 “ C ( X )| b ' 


(3.27) 
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The central angular momentum, taken about the mass center, is given by. 

a H cu = ±'tM{id)uU)b> + z l P™ x t, CM (3-28) 

»= 2 j=0 

where A p cu is a vector from the center of mass of the base body B to the system center of 

mass and is given as A p CK — x CM — go°i — 9x°2- 

Finally, the relationship between the derivative of momentum and the generalized ac- 

celerations can be shown to be: 


Lb i 


£}= o Mom ~ co 

Lb 2 

= 

£j=0 M lj«j-Cl 

HT • b 3 


£l =: 2 E]=o Mijiij - c i 


Once the equations of motions are derived, the kinetic energy is easily calculated as 

Kinetic Energy = i t Mu (3.30) 

Note that all the quantities of interest are given in terms of the mass matrix. From 
Equation 3.8, it is apparent that the mass matrix can be constructed with only 14 additions 

and 28 multiplications. 



Chapter 4 


Strategic Control 

This chapter describes the high level strategic controller. The strategic controller provides 
a means of performing task level commands. It provides an interface to low-level control 
laws which control physical states of the system. This chapter introduces the concept of 
state diagrams for designing and programming the strategic controller. An advantage of 
a strategic controller is that it allows for the rapid switching of control laws during the 
execution of a task. This is a very desirable capability for a system undergoing abrupt 
kinematic constraints. 


4.1 Introduction 

The classic digital control paradigm is depicted in Figure 4.1. This controller executes 
synchronously and serially, making it easily implementable on digital computers. These 
dynamic controllers are the heart of any robot control scheme, and are discussed in detail 
in Chapter 5. Unfortunately, dynamic controllers do not provide an easy interface to task 
level commands. Being error based, most dynamic controllers accept as inputs a desired 
state and not a desired task . Therefore, a higher level of abstraction is needed to interface 
desired tasks to desired states. The strategic controller is one such interface. Many of the 
ideas for strategic control come from the successful work of Schneider [27, 28] in applying 
strategic control to cooperative manipulation. 

By way of example, consider the task of “leaping across the table” as shown in Fig- 
ure 4.2. If an initial angle and velocity is given at the time of release, then the task is 
well defined. The reason this task is awkward to implement in a single dynamic controller 
is because discrete changes in the plant are occuring asynchronously. For example, when 
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Figure 4.1: Classic Digital Controller 

A block diagram of the classic digital controller. The inputs may not be 
in a convenient form for task level control. 

both arms release the bar, the number of degrees of freedom of the system are increased by 
four. Furthermore, tasks might require a PD controller, bang-bang controller, a variation 
of a computed torque controller, or some combination of the above. 


4.2 State Transition Graphs 

State transition graphs or state diagrams [10] are one approach to designing a strategic 
controller. In this context, one graphically divides a desired task into a set of phases or 
finite states. To complete a task or operation, states must be transitioned in a predefined 
order. The states need not be transitioned sequentially or only once, but can involve loops 
as shown in Figure 4.4. The transition between states can only occur in response to an 
external event or stimulus. The event and the current state uniquely determines the next 

state. 

For example, consider the state diagram shown in Figure 4.3. Each state, represented 
pictorially as an oval, must be transitioned in order for the robot to complete a LEAP 
maneuver. The arrows correspond to external events. The robot starts off in the “Idle” 






Chapter 4. Strategic Control 


41 


Leap 



Figure 4.2: Coordinated Leap Maneuver 

The figure shows the Pushoff ’ Rotation, and Catch phases for a straight 
back leap. The robot is going from right to left. 


state. Receiving a request to leap causes the robot to enter the “Windup” state. A 
“Ready” stimulus is sent (from a different process) when joint angles and joint rates are 
within predetermined tolerances, and the “Pushoff” state is entered. The system imparts 
momentum to itself by pushing off the bar along a predefined momentum trajectory. 

At this point two different events are possible. One possibility is that the system has 
achieved the desired final momentum, and so the “Coast” state is entered. The other 
possibility is that an error occured, such as a timeout warning or a gripper failing to open. 
In this case the “Idle” state is entered and the robot attempts error recovery. If all goes 
well, the robot “Tucks” in its arms to reduce its moment of inertia and rotates via the 
momentum wheel. Finally the system transitions between the “Approach” state and the 
“Catch” state when both grippers grasp the bar. After grabbing the bar, the system is 
brought to rest (zero momentum state) and remains in the “Idle” state. 

Another example is a crawl task, as shown in Figure 4.4. As one can see from these two 
simple examples, complicated locomotion tasks can be easily visualized by drawing state 
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transition graphs. 

Also, error conditions are easily incorporated into the task specification. More com- 
plicated error conditions and tasks can also be designed with this technique. The tasks 



described in the state diagrams axe implemented in a strategic controller by using state 
table programming. Table 4.1 is the corresponding state transition table for the state dia- 
gram shown in Figure 4.3. Each input or stimuli determines how the system will transition 
from one state to another. After receiving a stimulus, a transition routine 1 is executed 
which returns the value for the next state. For example, if the robot is in the “Idle” state 
and receives a “Leap” stimulus, the routine “CheckConfig” will be executed. If the robot is 
not grabbing the bar, it cannot perform a pushoff, and an error condition is detected. The 
routine returns the robot to the idle state with a warning message. If no error condition 
is detected, a windup trajectory is planned and executed. Upon successfully exiting the 
“CheckConfig” routine, the robot enters the “Windup” state. 

: In the actual implementation, each state is assigned a value. A transition routine is a C function that 
returns the value for the next state. 
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Figure 4.4: State Transition Diagram for a Crawl Maneuver 

Stimuli can come from a variety of external sources, but never from the strategic con- 
troller itself. They are, in fact, the input to the controller. In general, they originate from 
one of three places: 

1. command stimuli from the user, such as a Leap stimulus. 

2. helper stimuli, which come from external processes that monitor the system and 
report when a condition or event occurs. For example, a stimulus is sent to the 
strategic controller when the momentum reaches a desired value. 

3. hardware stimuli, such as watchdog timers. 

In the event that an erroneous or unexpected stimulus occurs, a default error handling 
routine is executed. 
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State 

Stimulus 

Transition Routine 

Next States 

Idle 

Leap 

CheckConfig 

WindUp 

Idle 

Windup 

Ready 

CalcLeapTrajectory PushOff 


PushOff 

MomentumMatch Release 

Cost 



Time 

SafeState 

Idle 



Range 

SafeState 

Idle 


Coast 

Release 

ArmTuck 

Tuck 

Idle 

Tuck 

Time 

PID Control 

Tuck 



Ready 

Activate Wheel 

Rotating 

Idle 

Rotating 

StopRotating 

SetApproach 

Approach 



Time 

SetApproach 

Approach 


Approach 

Grab 

GraspBar 

Catch 


Catch 

Time 

Dock 

Idle 



Table 4.1: State Transition Table 


4.2.1 Implementation 

The following code fragments show the data structure and declaration of the state table. 
All code is written in ANSI C using the GNU C compiler. 

#dsfine HAMELEH (32) 

#define MAXSTIMULI (16) /* Max stimuli branches in one state. */ 

#define MAXHEXTSTATES (5) /* Max next states for one stimulus. */ 


/* Data structure definition for a "state” */ 
typedef struct TaskStateTag { 
char name [HAMELEH] ; 
struct StimulusResponseTag { 

int stimulus; /* This entry* 8 stimulus */ 

int (*proc)(); /* The associated transition routine. */ 

struct TaskStateTag *nextState [MAXHEXTSTATES] ; /* The set of next 

* states */ 

> sr [MAXSTIMULI] ; 

> TaskStateStorage, *TaskState; 


typedef struct StimulusResponseTag 

*St imulusResponse , St imulusResponseStorage ; 
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/* Forward declaration ol the states */ 

static TaskStateStorage Idle.WindUp.PushOll .Coast, 

Tuck , Rotating , Approach , Catch ; 

Having defined the data structure, it is a simple matter to transfer the information 
from the state transition table into C. For the Leap task, the code looks like the following. 

/* Leap chain */ 

static TaskStateStorage Idle = {"Idle", 

{ 

{LEAP.STIH, CheckConfig, {ft WindUp, ftldle, ftReGrasp}}, 

{TIME.STIH, Sal estate, {ftldle}}, 

{DEMO.STIM , Thrusting, {ftCoast}}, 

{OUTOFRANGE.STIM, SaleState, {ftldle}}, 

{ 0 } 

} 

}; 

static TaskStateStorage WindUp = {"WindUp", 

{ 

{TIME.STIH, CalcLeapTra j ectory , {ftPushOfl, ftldle}}, 

■ {READY.STIM, Cal cLeapTraj ectory, {ftPushOll, ftldle}}, 

{OUTOFRAHGE.STIM, SaleState, {ftldle}}, 

{ 0 } 

} 

>; 

4.3 Trajectory Generation 

Once the strategic controller is programmed to perform a task, trajectories must be gen- 
erated for the dynamic controller. In order to implement a computed torque or inverse 
dynamic control law, the trajectories must be continuous in acceleration. Of course the 
term “trajectory” is used here in a very broad sense to describe a generic path. For exam- 
ple, one could generate joint trajectories, Cartesian endpoint trajectories, or momentum 
trajectories. 

Currently, all the trajectories in this research are generated by fifth-order polynomial 
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splines. There are theoretical [22, 37] as well as experimental [13, 8] studies which show 
that fifth-order polynomial give a good combination of slew time, smooth changes in ac- 
celeration, and minimum computation. One motivation for selecting fifth-order splines is 
that they are the set of functions that minimize the mean-square jerk 2 over the trajectory. 
Work by Flash and Hogan [8] suggests that humans instinctively follow minimum jerk tra- 
jectories while performing pick and place tasks. The following is a derivation to show that 
fifth-order polynomials do indeed minimize the mean-squared jerk. 

Let F be a function that has continuous second partial derivatives with respect to its 
arguments. We wish to find the function x(t ) that minimizes the expression: 

/= I' 1 F{t,x,x' ,x" ,x'")dt (4.1) 

Jto 

where x f denotes differentiation of x with respect to t. To minimize J, a necessary condition 
is for the first variation to vanish [11]* This can be written as: 


y* 1 rdF £ dF £ f dF ~ f/ dF __ n 

SI = L ‘aT + fe 7 fe 77 w 1 ' 

Integrating the last term by parts yields: 

« ■ r • > * s*-- ish: ■ • 

Integrating twice more by parts gives: 

r*i r dF d . dF , d 2 .dF. d 3 . dF » 

61 ~ J to ^dx ~ dt^dx^ + dt 2 dx" df 3 &c w 


+ 



ti r 

+ 

dx'" 

to 


{ ±1L _ °L )6x > 

{ Jj dx'" dx"' . 


1*1 


<0 


d 2 dF d dF , dF 

+ nZi> 6x 


dt 2 dx"' dt dx" dx 


(4.2) 
= 0 


Any function x(t ) that minimizes Equation 4.1 must satisfy the Euler equation 


dF_ _ _d £_ ( 6F_. _ 

dx dt [ dx' )+ dt 2 1 dx" } dt 3 1 dx'" ’ 
With three natural boundary conditions: 


0 


’ d dF 
' dtdx 



d 2 dF d dF dF' 
' dt 2 dx '" dt dx" + dx' ° X 


h 


t o 


<0 

tl 


to 


0 

0 

0 


(4.3) 


(4.4) 

(4.5) 

(4.6) 


2 Jerk is the derivative of acceleration. 
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Note that if the unknown function x(t) and its derivatives have prescribed values at the 
endpoints, the three natural boundary conditions are satisfied. 

We are now ready to solve the for the minimum jerk. This can now be stated as. 


minimize J = 



dt 


given x(0),x'(0),x"(0),x(f/),x'(f/), and x"(f/). 

Since the conditions at the endpoints are given, the three natural boundary conditions 
are satisfied. Therefore, the Euler equation becomes: 

■ 2 \ 

= 0 



d 3 x 


dt 3 


or 


d®x 

IF 

x(t) 


0 

a s t s + a 4 t 4 + a 3 f 3 + a 2 t 2 + M + ao 


(4.7) 

(4.8) 


which is the desired fifth-order polynomial. The coefficients u, are uniquely determined by 
the boundary conditions and are given by: 


ao 
a i 
a 2 

03 
a 4 
a 5 


x(0) 

x'(0) 

*"( 0 ) 

2 

(x"(i/) - 3x"(0))ty - (8 x'jtf) + 12x / (0))t/ + 20(x(</) - x(0)) 

2 tj 

-(2 x^jtf) - 3x"(0 )Jtj + (14x ; (f/) + 16xX0))tf - 30 (xjtf) - x(0)) 

2 1 ) 

WVjl ~ r"(0))*/ ~ 6(x , (t / ) + ^(0))^ + 12(x(f/) - x(0)) 

2t 5 f 


Momentum Trajectory Generation Momentum trajectories are used to specify the 
position and velocity of the mass center of the robot. For example, while the robot is at rest 
in the closed-chain configuration, it could be commanded to “move its mass center 0.15 m 
in the right”. Since forces are linearly related to the first derivative of momentum, a fifth- 
order polynomial is sufficient to specify the path of the mass center through momentum 
space. The coefficients of the polynomial are determined by specifying the elapsed time of 
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the slew, and the initial and final position of the mass center. Since the mass center is at 
rest at the beginning and end of the slew, all higher order derivative terms are set to zero. 

In general, one calculates a momentum trajectory by specifying a time interval, and 
initial and final momentum states. A momentum state consists of five vectors, the integral 
of Unear momentum, both Unear and angular momentum, and the derivatives of Unear and 
angular momentum. In two dimensional Cartesian space, this corresponds to eight distinct 
quantities to specify uniquely a momentum state. These desired momentum states cannot 
be chosen arbitrarily, for there are constraints on the system such as the location of the 

mass center when both arms are grabbing the bar. 

The vehicle performs a windup maneuver so as to accelerate the vehicle along the 
longest available path. The final momentum state at the end of the “windup” becomes 
the initial momentum state during “pushoff”. Empirically, it was found that the center 
of mass can move along a ray for 0.2 m (A x = .2) with a final velocity of 0.1 m/s. The 
trajectories were formulated such that the acceleration along the ray was constant. This 
assured that all the energy of the vehicle was directed toward imparting momentum to the 
system during “pushoff”. With these assumptions and a desired release angle 3 0, the final 
momentum state can be uniquely determined. Figure 4.5 is a pictorial representation of a 

momentum trajectory. 


4.4 The Switching Problem 

When the strategic controller receives a stimulus that an event occurred, such as a change 
in the number of degrees of freedom of the system, it should react to this discontinuous 
change in the plant in such a way as to achieve smooth transitions. The following descrip- 
tion explains why it should always be possible to achieve this transition with rigid-body 

manipulators. 

While in the closed-chain configuration, the number of degrees of freedom is reduced 
because of kinematic constraints. Differentiating the constraint equations gives a Unear 
constraint in the velocities, or generalized speeds, as was shown in Equation 3.14. Thus, 
it is always possible to compute a consistent set of generalized coordinates and generalized 
speeds for a rigid body system. When transitions occur, they must be continuous in 
position, and can only be discontinuous in velocity as a result of impulsive forces. This 
discontinuity in velocity can occur when the tips of the arms grab the bar with a non-zero 
a The angle 6 = 0 rad corresponds to a straight-back pushoff as shown in Figure 6.1. 
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Figure 4.5: A Momentum Trajectory 

Tie arrow represents a possible path of tie mass center. To move the 
mass center along a path, a fifth-order trajectory is generated from Equa- 
tion 4.8. The coefficients are determined by specifying tie position of the 
mass center , momentum, and derivative of the momentum at the two end- 
points of the trajectory. The position of the mass center is constrained by 
tie lengths of the arms, while tie velocity is constrained by link lengths 
and maximum torque capability 


velocity and decelerate to zero in “zero time”. Of course the state does not go to zero: 
only the velocity at each tip does. 

In the case of a “soft landing”, the relative velocity of the end effector and the target is 
zero, and so all the states are continuous during the transition. By calculating a consistent 
set of states for the full holonomic system, any controller will have the current estimate of 
the state. Thus, when switching from one control law to another, errors in configuration 
will not contribute to transient errors in the behavior of the system. 


4.5 The Strategic Controller 

The strategic controller is executed as a separate process by the real-time operating system. 
Most of the time it is blocked on a read, meaning that the processes is idle and waiting for 
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input. All inputs to the finite-state machine are in the form of stimuli. After a stimulus 
is received, a corresponding transition or subroutine is executed. These routines check on 
the physical configuration (open-chain or closed-chain) of the robot, and implement a new 
model of the system when necessary. The transition routines can also install new filters 
and controllers, as well as change gains in the dynamic controller. 

Although there are many advantages in this implementation, it is not without draw- 
backs and limitations. Because the finite-state machine runs as its own process, the con- 
troller has more than one thread of execution. This presents problems for real-time debug- 
ging and offline simulation. Also, the stimuli arrive asynchronously, yet must be incorpo- 
rated with synchronous control. Modern operating systems, such as VxWorks, provide a 
message-passing facility that performs the synchronization. 

4.5.1 Implementation 

The following code segment is an ANSI C version of the finite-state machine. When a 
stimulus is received, the finite-state machine compares the stimulus with a list of allowed 
ones for the current state. It then executes the transition routine, and moves to the next 
state based on the transition routine’s return value. 

static TaskState State; 
static int FSMxid; 
boolean FsmVerbose; 

static void FSMDriver(int stimulus) 

register boolean found = FALSE; 
register StimulusResponse sr; 
register int stimno, retcode; 
register TaskState lastState; 

lastState = State; 

stimno = 0; 


/* Look up the stimulus */ 
for (stimno =0; ; stimno++) { 
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sr = *State->sr[stimno] ; 
if (Br->stinniluB ** 0) { 
if(FsmVerbo8e) { 

print! ("FSMDriver: Unexpected stimulus %d.\n H , stimulus) ; 

> 

found = FALSE; 
break; 

> 

if ( (sr-> stimulus == stimulus) I I (sr-> stimulus == AIY_STIM) ) { 
found = TRUE; 
break; 

> 

> 

if (found) { 

retcode = (*sr->proc) (stimulus) ; /* Execute the transition routine. */ 
State = sr->nextState [retcode] ; /* Update the State */ 

> 

if (FsmVerbose) { 

printf ("FSM: */.s --y,2d--> ‘/.s\n" , lastState->name, stimulus, State->name) ; 

> 

> 


/* This daemon exists simply to weave the asynchronous stimuli into a 
synchronous stream. lote also that it allows the stimulus routines 
to run Unlocked, thus preventing unnecessarily long interrupt 
latency. */ 

static void FSMDaemon () 
int stimulus; 
while (TRUE) { 

readCFSMxid, Jkstimulus, sized (int)); /* Wait lor a stimulus. */ 

FSMDriver( stimulus) ; /* Process it. */ 
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> 

/* Sand a stimulus to the state machine. WARJIIG! 
re-entrant, as it is executed asynchronously by 
priority. */ 

static void SendFSHStimulus(int stimulus) 


This code MUST be 
routines ot varying 


write (FSMxid, ftstimulus, sizeot (int)) ; 


Chapter 5 


Dynamic Control 


This chapter describes the various dynamic controllers used in relocating and reorienting 
the robot. Because the system undergoes changes in configuration, different controllers 
are implemented at different times depending upon the state of the strategic controller 
described in Chapter 4. For locomotion, there are two basic quantities of interest to 
control: momentum and orientation. Other tasks, such as manipulation, control endpoint 
position or endpoint force. By formulating the equations of motion as shown in Chapter 3, 
the strategic controller can easily switch amongst a variety of control laws which regulate 
different quantities. The basic controllers discussed in this chapter are. 

1. PD and PID controllers to regulate joint angles. This is a robust but crude control 
law used to tuck in the arms while the robot is controlling its orientation. 

2. Momentum controller to cause the system to follow desired momentum trajectories 
discussed in Chapter 4. This controller is used to impart momentum to the system 
during a pushoff maneuver or to relocate the mass center during a crawl maneuver. 

3. Bang-Bang controller to reorient the attitude of the robot in minimum time. 

This chapter outlines a technique for controlling the momentum of a robotic system. 
This is accomplished by causing the linear and angular momentum to follow desired momen- 
tum trajectories in the presence of kinematic constraints, which occur in closed-kinematic- 
chain configurations. This powerful idea facilitates a simple interface between the high-level 
strategic controller and the low-level dynamic controller that is “exact” in that it compen- 
sates for all dynamic forces. This control paradigm does not involve inversion of the mass 
matrix and is amenable to parallel processing, even though the resulting system is not mod- 
eled as an “in-parallel system”. The system momentum controller has been implemented 
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on a free floating robot to perform snch maneuvers as pushoff and catch. 

In addition, this chapter discusses some of the issues involved in orientation of a free- 
flying multi-link system of rigid bodies. While the robot is free-flying, the momentum 
is uncontrollable using internal torques and forces as a consequence of the principle of 
conservation of momentum. However, conservation of angular momentum results in non- 
holonomic constraint equations. It is through these equations that the orientation of the 
robot can be controlled. 


5.1 PD and PID controllers 

Proportional-derivative (PD) and proportional-integral-derivative (PID) control is the clas- 
sic standard control law by which new control .laws are judged. Any text on feedback 
control such as Franklin and Powell [9] will describe various design techniques for choosing 
the velocity and position gains. Its major advantages are simplicity in implementation and 
guaranteed stability for passive systems (linear and nonlinear) in the presence of parame- 
ter variations. The disadvantage is that no information on the model is incorporated into 
the controller, and so performance is usually not as good as with other methods. More 
importantly, it is difficult to incorporate abrupt kinematic changes into a PD design. To 
accomplish this, a new controller, called system momentum control was developed. 


5.2 Computed- Torque Control 

Computed- torque control or inverse-dynamics control is one type of nonlinear controller 
design that has gained popularity for control of rigid robot manipulators. In implementing 
a computed torque controller, the control law is decomposed into two parts: a model- 
based control law that linearizes and decouples the plant equations, and an error-driven 
or servo control law that is formed by differencing desired and actual values [6]. A more 
general approach to linearizing control of nonlinear systems can be be found by Sastry and 
Bodson [26]. The following sections describe three different variations of computed torque: 
joint-based computed torque, Cartesian-based computed torque, and system momentum 
control. 

Joint-based computed torque For any n degree of freedom rigid-body manipulator, 
the dynamics have the form: 


Tr = M{q)u-c{q,u)~ f{q,u) ext „ nal 


(5.1) 
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u = Y(q)q 

where the inertia matrix is M G i? nXn , c € R n is a vector of centrifugal and Coriolis 
terms, and / external € R n is a vector of other forces such as gravity, friction, and external 
disturbances. The problem of controlling a nonlinear system such as Equation 5.1 can be 
accomplished if one partitions the control law into two parts: 

r = ar' + /3 


where r e R n is a vector of joint torques. By choosing 

a = T _1 M ( 5 - 2 ) 

13 = — T -1 {c + f external} ( 5 ‘ 3 ) 

The servo law becomes 

q c = 9(f + K v {q d - q) + K p (q d - q) ( 5 - 4 ) 

r' = Yq c + Yq ( 5 - 5 ) 

where q c is the commanded acceleration to the control law and q d is the desired accelera- 
tion. In the absence of any trajectory error, these two quantities would be equal. Figure 5.1 
shows a block diagram of the computed-torque control scheme. If the state is chosen to 
be q and q, with the forces acting at the joints, and T = Y = I, then Equations 5 . 2 - 5.5 
become the familiar form found in Craig [6]. 


Example: Joint-based computed torque for two arm floating robot This exam- 
ple shows how to design a computed torque controller for a two arm robot on a floating 
base. In this case, there are 8 position and velocity states, but only 5 motors. By par- 
titioning the state into controllable and uncontrollable parts, one can resolve the torques 


necessary to control the quantities of interest as follows. Let: 
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Figure 5.1: Computed Torque 

Block diagram of a computed torque controller for a general rigid body 
robot. Dashed lines represent a nonlinear relation between input and 

output. 

The above equation is just a reordering of the rows of the equations of motion. By 
rearranging terms, the vector ii u can be solved for in terms of measured or desired quan- 
tities. 

Uu = Mil [c u + T u t - (5-7) 

By recognizing that the term is zero for the free-flying LEAP system, the 

computed torque equation becomes: 

r = T~ l {[Mcc - M^M-J Af£> c + - c c ]} (5.8) 

Everything is known to calculate r except tt c . To calculate u c let: 

itc = Y c { q d + K v (q d - q c ) + K p (q d - q c )} (5-9) 

The major difference between a fixed-base and free-flying robot is the singularity of 
the T matrix. In the case of free-flying robots, the momentum of the system cannot be 
controlled by internal motors, and so the number of actuators is less than the number of 
states. If one uses thrusters to control momentum, the two cases can be treated the same. 
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Cartesian-based Control In many situations, one is interested in specifying a trajec- 
tory of the tip of a manipulator, and not the motion of the joints themselves. In this case it 
is more convenient to formulate the control law in Cartesian space instead of Joint Space. 
Such a control is also called Operational-Space Control [1, 18]. 

Starting from the basic equations of motion given in Equation 5.1 

Tr = M(g) u - c(g, u) - /(g,«)ext e rnof 
r = J(q)u ( 5 - 10 ) 

where v £ R n is a vector of translational and rotational velocities of the end-effector in 
Cartesian space and J{q ) € R nXn , called the Jacobian, is a matrix that relates gener- 
alized velocities u in joint-space to Cartesian-space velocities v at the end-effector. By 
differentiating Equation 5.10 and substituting into Equation 5.1, 

a = v = J(q, u)u + J(q)u 

= j(q,u)u + J(q)M- l {q){c(q,u) + f{q,u) exieTnal + TT} (5.11) 

where a is a vector of accelerations at the tip. Note that the derivative j(q,u) of the 
Jacobian is a function of both q and u because of the linear dependence between q and 
u. The commanded torques at the joints can be derived by substituting for a the desired 
endpoint accelerations a 4 . 

t = T- 1 M(q)J~ 1 {q ) {a d - J(q,u)u} - T " 1 {c(g,u) + f(q,u) exteTnal } 

The Jacobian is generally invertable except where the manipulator approaches a sin- 
gular configuration. For a redundant manipulator, the solution is not unique, and further 
constraints or objectives can be imposed upon the system to uniquely determine the re- 
quired torques to cause the desired tip accelerations. 

5.2.1 System Momentum Control 

The momentum of a system of bodies is a well defined quantity that is easily calculated 
[15]. Being a function of velocity, mass and moment of inertia of the system, the calculation 
of momentum (both linear and angular) does not change with kinematic constraints. In 
the case of a free- flying robot, the total momentum is constant or conserved while floating 
freely in space, but is not conserved while connected to the space station 1 . The idea behind 
system momentum control is to cause a multi-body system to follow a. momentum trajectory 
ijt is assumed that the space station is so massive that it appears to the robot as mertially fixed. 
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until a final desired momentum state is reached. At that point the system changes from a 
closed-chain configuration to an open-chain configuration (releasing from the space station) 
with the desired final momentum. Momentum control differs from traditional robotic 
_ control policies in that instead of controlling one or more state variables (position or velocity 

of a joint or tip), it forces the momentum of the entire system to follow a commanded 

momentum trajectory as described in section 4.3. 

The controller consists of two parts. The first part combines the dynamical relationship 
between desired momentum and the generalized accelerations ( u terms) with the closed- 
kinematic-chain constraints. This is analogous to the servo part of the computed torque 
controller. Having solved for a consistent set of generalized accelerations, the joint torques 
are then computed. 



Momentum Error Equation Given a momentum trajectory as outlined in 
an error equation can be written as: 

Chapter 4, 


L x = L Xd + K M x {L Xd - L x ) + K i x J {L Xd - L x )dt 

(5.12) 


L y = i Vd + K Mu (L yd -L y ) + K Iv J{L yi -L y )dt 



HT = H C , d +K M ,(H™-H<n 

(5.13) 


where the subscript “d” denotes desired quantities, L is the linear momentum and H is 
the angular momentum expressed in the inertial frame. The momentum gains Km de- 
termine the placement of the closed-loop poles, while Ld corresponds to the feedforward 
term. Note that Equations 5.12-5.13 describe type 1 error equations in momentum [24, 9], 
and cannot take out steady-state errors in position 2 . By adding another integral term to 
Equation 5.12, the system will exhibit zero steady-state error in position. Steady-state 
errors in position of the mass center arise when air tubes and wires running through the 
robot exhibit a constant spring force on the arm joints. Equation 5.13 does not have an 
integral term, because angular momentum gives rise to nonholonomic constraint equations. 
Nonholonomic equations require path dependent integration, and are not analytically inte- 
grable. Since orientation can be controlled independently of angular momentum, the total 
linear momentum of the system was controlled to be zero. 

Resolving Generalized Accelerations Having determined the commanded momen- 
tum, the next step is to resolve the generalized accelerations. This is accomplished by 

2 The integral of linear momentum can also be expressed as the total mass times the position of the mass 
center. 
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substituting Equation 3.15 into Equation 3.29 and solving for u T as follows. 
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Like the previous methods of computed torque, system momentum control is another 
way of finding or specifying a consistent set of generalized accelerations. 

Solving for Joint Torques 

Having derived a consistent set of commanded accelerations, the last step in the control 
law is to resolve the joint torques. It is quite common for the solution to this problem to be 
overconstrained. This means that there is an infinite set of allowable torques that will give 
the desired accelerations. To find an unique solution, additional objectives must be imposed 
on the problem. One objective that is commonly used is to minimize the 2 norm of r. For 
autonomous vehicles with limited battery supply, this objective corresponds to minimizing 
the total power consumed. Other objectives include minimizing the 1 norm or the infinity 
norm, which correspond to minimizing total current and peak torque respectively. For low 
order systems, it is very computationally efficient to solve the least squares problem for r 
using Equation 3.18. 


5.3 Practical Issues With Computed Torque 

In the derivation of computed torque controllers, certain assumptions were made that 
impact the design and performance of the system. The following paragraphs describe the 
two major limitations: continuous time derivation and knowledge of the entire state. 

Sample Rate and Computational Delays In all the derivations for computed torque, 
it is assumed that the plant and control law are running in continuous time and that there 
is zero time delay for computation. At first glance, it is unclear what the impact this 
assumption has on the stability and performance of nonlinear systems. The answer to 
these questions depends on the type of system, the complexity of the system, the actuator 
authority, and the available computer power. For example, Uhlik [36] has experimentally 
shown that for a fixed-base two-link arm with elastic drives, a sample rate of 50 Hz was 
insufficient to stabilize his system with a desired closed-loop bandwidth of 2 Hz. The 
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problem stems from the fact that the system needs to be sampled at 30 to 50 times the 
highest natural frequency to insure stability. Because fixed-base robots do not suffer to the 
same degree as space robots for weight and power constraints, terrestrial high-performance 
systems can be built where actuator authority is practically unlimited. For example, in the 
experiment described in [36], applying maximum torque to the shoulder or elbow motors 
would permanently deform the springs in the drive-train. Therefore, his system was not 
actuator limited. This can be contrasted with a space-robot, whose acceleration will be 
small compared to peak motor torque during a “push-off” maneuver. 

For rigid body systems, Wampler [37] asserts that one needs to sample at 30 to 50 times 
the desired closed-loop bandwidth . For a bandwidth of 1 Hz, this amounts to a sample period 
of 20 to 30 milliseconds. Of course bandwidth is not necessarily the criterion one should 
use in evaluating control performance. For linear systems, closed-loop bandwidth denotes 
the ability of the system to track sinusoidal inputs. It can be defined as the input frequency 
at which the output amplitude is 3 dB below that of the input amplitude. This definition 
is a bit arbitrary, and one needs to question its relevance in selecting sample rates. 

The problem is that the definition of bandwidth assumes a linear system and therefore 
does not account for nonlinear effects such as actuator saturation. For small- amplitude 
inputs, the “closed-loop” bandwidth could be much higher than for high-amplitude inputs 
due to saturation. The need for good disturbance rejection can also drive a design to high 
closed-loop bandwidth. Yet in space, external disturbances will be small in magnitude and 
frequency content. Therefore, the issues that drive sample rates are not bandwidth, but 
actuator authority, desired tasks, and system dynamics. One should sample 30 to 50 times 
the largest desired/ achievable frequency content of the states. 

Lack of Knowledge of the Plant and State The second assumption in using computed- 
torque control is knowledge of the entire state. This requirement is not as much an issue 
for robotic manipulators, since most of the states are single degree of freedom joints. The 
joint positions can be accurately measured with optical encoders 3 or RVDT sensors, and 
rate information can be inferred from these signals or estimated using extended Kalman 
filters (EKF). The hardest states to measure are the orientation and position of the base 
body. Global positions are measured in the laboratory using an offboard vision system. 
In space, inertial navigation systems using laser gyros and accelerometers combined with 
differential GPS could give centimeter accuracy in base position. 


3 Commercial encoders can measure angles of 77/i radians and angular velocity ranging from 0.07 to 4.0 
radians/sec to a few percent accuracy. 
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5.4 Bang-Bang Control 

During free- flight, a bang-bang control law was used to perform large angle slews. This 
control law is optimal in the sense that it produces the minimum time solution to a linear 
system with limited actuator authority. The derivation is well known (See Bryson [3]), and 
only the results are given here. If 75 is the torque applied by the momentum wheel motor, 

then . r % 

Tmax j • , fa\<l2\T m ax I 

r 5 = j-sgn <j <72 + y 7 j 

where —T max < r 5 < T max and I is the effective moment of inertia about the system center 
of mass. To prevent chatter after reaching the final attitude, a simple PID control law was 
employed to regulate the attitude. In the phase plane, this control law will take the robot 
from any initial angular velocity and angle to zero angular velocity and angle by applying 
full torque (either positive or negative) and switching at the appropriate time to full torque 
in the opposite direction. 


5.5 In-Parallel Systems 

Consider the two systems shown in Fig 5 . 2 . Both drawings show six-bar linkages that form 
a closed-kinematic-chain configuration. The major difference is the location of two motors, 
which in one case is inertially fixed, and in the other body fixed on a free-floating target. 
Fig 5 . 2 a is called an “in- parallel system”, because cutting the chain at points “A” and “B 
results in two independent two link manipulators. Schneider [27] has taken advantage of 
this configuration to design a parallel control algorithm that implements object impedance 
control. Specifying desired accelerations and measuring the external forces at the tips 
uniquely determines the applied torques to the motors. 

This same method does not work in Fig 5 . 2 b. If the links are cut at points A and 
“B”, the resulting Jacobians are singular for the two arms because the torque from a motor 
cannot exert a force at zero distance, but only at a finite distance away. Therefore a cut 
cannot occur at a motor. If the links are cut at points “C” and “D”, the two shoulder motors 
are still coupled through the dynamics of the floating base. Systems can be decoupled if 
the cuts result in topological tree structures whose roots are inertially fixed and whose 
Jacobians are not singular. 

Finally, the position or velocity of an object or a link is not important for locomotion, 
but rather the position and velocity of the system of bodies. For this reason, a different 
approach will be taken to control kinematically coupled systems of rigid bodies that does 
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Figure 5.2: In-Parallel Systems 

The location of actuators determines how easily a closed-kinematic-chain 
system can be parallelized. 


not involve taking advantage of an in-parallel system, but rather incorporates the kinematic 
constraints with the dynamic equations of motion. 




Chapter 6 


Experimental Results 


In this chapter, experimental results are presented for the momentum controller whose 
design is described in Chapter 5. These results are intended to verify the design and 
predicted performance. A comparison is made between simulation and experiment to see 
how well the model predicted the actual behavior of the robot. 

This chapter is divided into two sections: results for an entire LEAP maneuver, and 
results for joint PD, bang-bang control, and momentum control. 


6.1 An Entire LEAP Maneuver 

Figure 6.1 is a montage of an actual LEAP maneuver. The robot was commanded to leap 
straight-back a distance of 3.66 meters, and to complete this task without using thrusters. 
To accomplish this, the strategic controller guided the robot through the various phases of 
the maneuver as described below. 

Upon receiving the command to leap, along with a desired final momentum state, the 
robot entered the “Wind-up” phase. After the w Wind-up” phase, the robot accelerated 
along a straight path, releasing the bar when the velocity of the mass center reached 
100 mm/s. 

After releasing the bar, the robot brings its arms into a tuck position and rotates 
180 degrees . During this period, no attempt is made to control the momentum of the 
system or to correct for errors in location of the mass center. The angular position of the 
base is controlled using bang-bang control (minimum time slew) with feedback from an 
onboard angular rate sensor to a momentum wheel. When rotation is complete, the arms 
are extended to prepare for landing. 

Finally, when both arms grasp the bar, the robot is commanded to stop by implementing 
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Leap 



Figure 6.1: Coordinated Leap Maneuver 

The figure shows the Pushoff, Rotation, and Catch phases for a straight 
back leap . The robot is going from right to left. 


an error equation in momentum only. When the momentum is below a specified threshold, 
the robot returns to its ready position for another task. With the current experimental 
setup, the robot can position itself to within 70 mm over a 3.66 m distance without the 
need for midcourse correction. 

Various other angles of departure, between 0 deg and 15 deg were tried. In each case, 
the robot landed to within 70 mm of the desired location. Limitations in the experimental 
setup prevented executing launch angles beyond 15 deg. Due to large asymmetries in 
off-axis pushoffs, one arm imparts the majority of the momentum to the system. With 
the current link lengths and peak torque capabilities of the motors, a release velocity of 
100 mm/s or greater is not possible for departure angles beyond 20 deg. For slower release 
velocities, grounding effects in the air bearing dominate the experimental results. Of course 
leaping is but one form of thrusterless locomotion. To translate sideways, crawling might 
be a preferred method. 
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6.2 Joint PD and Bang-Bang Control 

Although this research focuses on combining different control laws to perform a high level 
task, each subtask needs to be experimentally verified before the system as a whole can 
be evaluated. Figure 6.2 shows a plot of the right-shoulder and right-elbow joints going 
through commanded 73 degree and 95 degree slews. This maneuver is identical to an arm 
tuck, which the robot performs to minimize its moment of inertia. The control law is a 
simple PD error law wrapped around each joint. 

Although it insures stability, the PD control law is not designed to give high perfor- 
mance for a nonlinear system. No attempt was made to model the system or to compensate 
for steady state errors. The steady state errors can clearly be seen in Figure 6.2 as the 
difference between desired position (the dashed lines) and the measured position. A PID 
control law was also designed which eliminated the steady state error. Since the only 
function of this control law was to move the arms into a tuck position, great accuracy 
in position and trajectory following was not necessary, and the performance was deemed 
adequate. 

The desired trajectories are formulated as fifth-order polynomials to ensure smooth 
transitions in angular acceleration as well as in angular velocity and position. 

9 d (t) = a 5 t 5 + a 4 t 4 + a 3 f 3 + a 2 t 2 + a t t + a 0 

= 5ast 4 + 4a 4 t 3 + 3 a 3 t 2 + 2a 2 t + ai 
8 d {t) = 20a s f 3 + 12a 4 t 2 + 6a 3 f + 2a 2 

If the initial and final angles are 0, and 0/, and tj is a desired elapsed slew time, then the 
six unknown coefficients a, are given by: 

0, 

0i 

0 1 

2 

(0/ - 3 0i)t 2 f - (80/ + 120,)*/ + 20(0/ - 0.) 

2t) 

-(20/ - 3 Bj)t) + (140/ + - 30(0/ - 0,) 

2t) 

( 0 / - 8i)t) - 6 ( 0 / + 6i)t f + 12(0, / - Oi) 

2t) 


do = 

ai = 

a 2 = 
a 3 = 

a 4 = 

05 = 


It is important to specify acceleration profiles on lightweight arms to prevent the jerkyness 
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Figure 6.2: Nominal Slews of a PD Controller for a Two Link Arm 

A series of 3.5 second slews were commanded to each joint separately. 
The dashed lines represent desired values, while the solid lines show ex- 
perimentally measured data. While position tracking is good , velocity 
tracking is rather poor. Coupling between joint results in large variations 
in torque (bottom plots). 
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associated with large changes in acceleration. Actually, humans tend to follow minimum 
jerk 1 trajectories while performing pick and place tasks [8]. 

The base orientation was controlled by a different method: the time-optimal bang-bang 
controller. Both angular position and angular velocity of the base were used to determine 
whether the momentum wheel motor should supply maximum positive or maximum neg- 
ative torque. Figure 6.3 shows the orientation and angular velocity for a 180 deg slew. 
During the slew, the arms were regulated about a 45 deg angle. The degradation in the 
measurement results from chatter while the controller is regulating about zero. For a sys- 
tem with a small T max /I (See section 5.4), the peak accelerations are small; inertia acts like 
an acceleration filter. Therefore, the system does not experience jerky motions between 
maximum changes in input torque. The T ma x/I ratio is 0.164 rad/sec 2 in the nominal tuck 
configuration. In an actual leap maneuver, the bang-bang control law is switched off after 
reaching the desired angle, and a simple PD controller is installed to regulate the final 
orientation of the base. 




Time (s) 

Figure 6.3: Orientation and Angular Velocity of the Base for 

Minimum Time Slew 


1 Jerk is the derivative of acceleration. 
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6.3 Momentum Control 

Figure 6.4 shows the response of the system to a step command in the position of the mass 
center. This test was performed to experimentally verify that the feedback part of the 
control law caused the system to behave like a second order system with damping ratio 
C = 0.8 and natural frequency u = 1.7 rad/sec. The slight deviation between experimental 
and predicted behavior is due to unmodelled friction effects in the air-tubing and wiring 
that run through the arms. Figure 6.5 shows a comparison between desired, simulated and 


Step Response 



Figure 6.4: Response To Step Input 

Plot of the center-of-mass position to a commanded step input. Also 
plotted is the ideal response of a second order system to a step input. 
This plot shows excellent agreement between the theoretical and actual 
error-equation pole positions (dashed vs. solid line). This plot demon- 
strates that under momentum control , tie system will respond to errors 
in trajectory tracking like a second-order system. 


experimentally measured positions and velocities for the mass center during the windup 
and pushoff phases. The steady-state error in position at point B, the end of the windup, 
is due to the air tubes applying a torque to the elbow joints. Since there is no integral error 
in position, the result was a small steady state error. However, no such error occured in 
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momentum, since there is integral error in momentum, and the system tracked the desired 
momentum trajectory well. At point C, the robot released the bar and floated to the other 

side of the table. 







Chapter 7 


Extensions to Three Dimensions 


Thus far, most of the discussion has focused upon locomotion in two dimensions. Although 
laboratory experiments were performed in two dimensions, many of the ideas easily extend 
to three. The derivation of the equations of motion, the design of the strategic controller 
and the momentum controller readily extend to three dimensions, and require no modifi- 
cation to the theory. Issues that do need to be addressed involve orientation and vehicle 
design. 

This chapter is divided into three sections. The first section deals with an abstract 
treatment of orientations of deformable bodies. By using purely geometric arguments, the 
net rotation can be calculated from a set of path dependent motions. The second section 
describes a method of performing attitude control using a fifth-order trajectory in Euler 
parameters and resolved acceleration or computed torque. The last section describes some 
design considerations when going from two to three dimensions. 


7.1 Orientation in 3 Dimensions 

Although the total angular and linear momentum of an object is constant in the absence 
of external forces and torques, the orientation of that object need not be constant. Studies 
have been done [17, 14] which demonstrate arbitrary orientation with zero angular momen- 
tum. A more general treatment of the subject was done by Shappere and Wilczek [30], 
which presented a kinematic formulation of this problem in terms of a gauge structure over 
the space of shapes that a body may assume. Their work is presented here along with 
some comments on how it might be applied to three dimensional robotic design. 

Consider the set of all possible shapes of a deformable body, which we will call con- 
figuration space. Each point in this space corresponds to a unique shape with orientation 
and position of the mass center. This set can be partitioned into equivalence classes by an 
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equivalence relation; shapes that differ only in center of mass location and orientation are 
equivalent. When no external forces act on a body, the center of mass is unchanged for 
every shape in configuration space. Thus we will work in the center of mass frame, where 
the space of located shapes is just the space of unlocated shapes with orientation. 

An example will help to illustrate some of these ideas. Consider a deformable body 
consisting of a hinge, separated by an angle 0, where 0 < 9 < x. An equivalence class of 
unlocated shapes is the set of all shapes with the same angle 9 as shown in Figure 7.1. 
The question we wish to answer is: what is the net rotation or change in orientation that 



Figure 7.1: Equivalence Class of Unlocated Shapes 

The set of possible shapes can be partitioned into equivalence classes of 
shapes that differ only in orientation. The shapes in the boxes were chosen 
to be the standard shapes for that class. 


results when a deformable body goes through a given sequence of unoriented shapes in the 
absence of external forces or torques? The net rotation can be computed by making use 
of the law of conservation of angular momentum. This nonholonomic constraint will be 
sufficient to determine fully the net rotation of a deformable body. 

Suppose one has chosen a set of standard body-fixed reference axes for each possible 
unoriented shape. This is accomplished by choosing one shape from each equivalence class 
and assigning an arbitrary dextral set of x, y, z axes. Therefore, a standard shape is one 
point of the equivalence class of located shapes corresponding to an unlocated shape. In 
our example, the shapes inclosed in a square box comprise the set of standard shapes. For a 
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given sequence of these standard shapes S 0 (t), we wish to find the corresponding sequence 
of physically oriented shapes S(t ), which are related by the rotation matrix R{t)- 

S{t ) = R{t)S 0 (t) C 7 - 1 ) 

where R(t) is a 3 x 3 rotation (direction cosine) matrix which depends on the reference axes 
for S 0 (t). Of course the path of physical shapes S(t) should be invariant to the arbitrary 
choice of standard shapes. If one were to choose a different set of standard shapes: 

So = fi[S 0 ]S<, ( 7-2 ) 


then the new rotation matrix would be: 


R(t) = RWQ-'iSoit)] 


(7.3) 



to insure that S(t) is unchanged. One can write a differential equation in 

R(t) by defining: 



(7.4) 


dt L dt J 



es. 

II 

o ^ 

(7.5) 


The matrix A can be thought of as an operator on R which maps an infinitesimal defor- 
mation of S 0 (t) into an infinitesimal rotation. It can be shown that A , also known as the 
angular velocity matrix is skew symmetric, meaning that only 3 of the nine elements are 
independent. This fact will become very useful in subsequent calculations with A. Equa- 
tion 7.5 is actually invariant under arbitrary time rescalings, suggesting that one should be 
able to write Equation 7.5 in a completely geometric form. To see why this is so, consider 
a mapping t — * f{t). Then dt —* fdt while A — *• A// since: 


A(/(t)) = R-\m) 


- 1 1 = R~ l (f(t)) 


dt 


dR(f(t) ) 

dm 


/ 


A(f)/f = R-\f) 


dR{f) 

df 


Dynamics of a free body The dynamics of a body in the absence of any external forces 
or torques is completely determined by the conservation of momentum laws. Without loss 
of generality, consider the total angular momentum to be zero. 

If one considers a body consisting of a collection of N point masses m 1 ...m N at 
positions x 1 . . .x N , then the total angular momentum of the system is: 

H^e^m^xViP 

n= 1 


(7.6) 
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KJ 


where e,j* is the permutation symbol in three dimensions with i, j, k taking on values 1, 
2 or 3, and: 


=0 if two suffixes are equal. 

(i j k = i if ijk is an even permutation of the sequence 123. 

€i - k — _i if ijk is an odd permutation of the sequence 123. 

An identity which is useful for tensor cross products in three dimensions is [34] 

(■mra^mpq ~ ^rp^tq ^rq^ap 


(7.7) 


where Sij is the Kroniker delta taking the value 1 when i = j and 0 otherwise. 

Recall that the unlocated shapes are related to the oriented shapes at time t by the 
relation: 

x< n >(t) = R(t)xW(t) (7-8) 

where R(t) is a 3 X 3 rotation matrix. Substituting Equation 7.8 into Equation 7.6 gives 
an expression of the angular momentum in terms of and R{t)- 


N 


Hi = *ijk X 


m 


(») 


n=l 


Rjtx\ n) Rh 


+ Rjl%\ ^ Rkm’’ 


( n )" 

m 


(7.9) 


By setting Hi = 0 in Equation 7.9, multiplying by R~ 2 and performing some algebra, one 
obtains: 

A(t)ij = (R~i R)ij = -«jkVk = eijkluHi ( 7 - 10 ) 

where / is the inertia tensor of standard shapes S 0 (t ), and L is the apparent angular 
momentum of S 0 (t) at time t: 


iu £ ( 711 > 

n=l 

B, S *J*>il*> (712) 

n=l 

where use has been made of Equation 7.7 and the fact that A(f) is skew symmetric. Equa- 
tions 7.10 and 7.5 provide a complete solution to the problem of determining net rotation 
of a deformable body. 

For a multi-body space robot, the calculation of A can be computed in a more con- 
venient form by making use of Equation 3.24. Notice that this equation is linear in u,, 
the generalized speeds. If one orders the U{ such that the first three generalized speeds 
correspond to the measure numbers of the angular velocity vector given in Equation 7.24, 
then 


Hi = lijUj + /,„u„ 
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where t,j= l,2,3andn = 4...JV where N is the number of degrees of freedom of the 
system. If the angular momentum is zero, then 


— Tj m -fmn u n 

0 — UJ 3 u>2 

A(t) = u> 3 0 -u> i 

— 0^2 0 


(7.13) 

(7.14) 


7.2 Attitude Control in 3 Dimensions 

In spacecraft attitude control, the equations of motion are usually linearized about a fixed 
desired orientation. This is a very good assumption while regulating about a fixed attitude, 
and the literature is full of examples on how to implement 3-axis attitude control. The 
problem becomes more complicated when performing rapid slews from one orientation to 
another. Typically, the satellite is rotated about each principal axis independently, until 
the desired final orientation is achieved. In this way both the kinematics and the dynamics 
are decoupled into a set of linear equations. This technique presupposes that the torque 
vector is aligned with a principal axis of the spacecraft and that all other cross terms in 
the angular velocity vector are zero. 

In orienting a space robot, the problem is complicated by the fact that the principal 
axes change with configuration. Therefore, one should not expect momentum wheels or 
other torque devices to align with any particular geometry of the robot. Even if the 
torque vectors were aligned with the principle axes, one would like to specify an initial and 
final orientation, and have the robot reorient itself in one smooth continuous maneuver. 
One method proposed by Dwyer [7] transforms the equations of motion to exhibit linear 
input-output behavior. The following section outlines a scheme for general 3 dimensional 
attitude control of a free-flying robot. By combining this method with system momentum 
control, the user has an unified approach to thrusterless space robotic locomotion in three 
dimensions. 

Consider the problem of rotating a rigid body from an initial orientation O, to a final 
orientation Of. A theorem due to Euler on rotation states that every change in relative 
orientation of two rigid bodies can be produced by means of a simple rotation [16]. Let A 
be a unit vector parallel to the axis of rotation, and 6 be the angle of rotation. Then four 
scalar quantities, e \ , . . . , 64, called Euler parameters 1 , can be defined as: 



’The Euler Parameters should not be confused with the permutation symbol e,,k of Section 7.1 
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A , 

a = C • a; = £ • Oi 

A 


(*=1,2,3) 


e 


U = cos - 


(7.16) 

(7.17) 


where 01 , 02,03 is a dextral set of orthogonal unit vectors fixed in inertial space, with 
hi,h 2,&3 fixed in the body reference frame. The Euler parameters are not independent, 
but must satisfy the condition: 

The Euler parameters can also be expressed in terms of direction cosines as follows: 

(7.19) 

(7.20) 

(7.21) 


Cij = o,- 6 j (*,j =1,2,3) 

C32 — C23 


£4 = -(1 + Cn + C22 + C33)^ 


«1 = 
£2 = 
C3 = 


4 £ 4 

C 13 — C 31 
4£ 4 

C*21 ~ C 12 

4f4 


If one defines the angular velocity vector A u> B of B relative to A as: 

A u> B = uibi + CJ2&2 + ^363 


(7.22) 

(7.23) 

(7.24) 


then the relation between the measure numbers w; of angular velocity and the derivatives 
of the Euler parameters can be given as: 


A r 1T 

c — L £1 C2 C3 c 4 1 


A r 

w = [ 


(7.25) 


and 


E = 


[ Wi 

U>2 IjJ 3 

o] r 

(7.26) 

u - 

-£3 £2 

Cl 


f3 

£4 -£1 

C 2 

(7.27) 

-f 2 

Cl c 4 

C3 


-Cl * 

-€2 — C 3 

C4 _ 


i = 

l E w 


(7.28) 

u = 

2E T i 


(7.29) 
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The derivations for the above equations can be found in [16]. Two other quantities of 

interest are the derivatives of Equations 7.28 and 7.29 which will be needed for control. 

They are: 

€ = i(Eu> + £d>) (7-30) 

u = 2 (E T i + E T e) (7-31) 


The last equation can be written in a slightly more compact form as: 


Ji 

CJ2 

4 J 3 


= 2 


£4 

£3 

62 


^3 

64 

-61 


“62 

61 

64 


“61 

“62 

“63 


61 

6*2 

63 

64 


(7.32) 


7.2,1 Desired Trajectories in Euler Parameters 

Consider the task of reorienting a rigid body from an initial orientation and angular velocity 
to a final orientation and angular velocity at some time tf later. One could easily cause 
the path to follow a fifth order trajectory such as: 

€i d (t) = dts* 5 + aj 4 t 4 + aftt 3 + a f * 2 t 2 + d{\t + a;o (* — 2, 3) (7.33) 

where the a»j would be determined by the initial and final conditions as described in 
Appendix 4.3. The fourth Euler Parameter, e 4 can be solved in terms of the other three 
parameters as follows: 


£4 

= \j 1 - £? - c 2 ” c 3 

(7.34) 

u 

= — (ci€l + £2*2 + £3£3)/ c 4 

(7.35) 

£4 

= — (fie'i + £2 £2 + £3«3 + £i 2 + £2 2 + c 3 2 + f 4 2 )/ £4 

(7.36) 


where e 4 ^ 0. Should c 4 = 0 and e 4 ^ 0, then 2 : 

€ 4 2 = -(6i€i + €262 + 6363 + 6*x 2 + 6*2 2 + 63 2 ) 

6 4 = “ ( 3 (€i€i + €262 + 6363) + 6iCj 3 ^ + 62 Cj ^ + 63C3 ^)/ 3 c 4 


7.2.2 Error Equation in Euler Parameters 

Similar to the error equation in momentum, an error equation can be written in terms of 
the Euler Parameters. By substituting the values of € and u> into Equation 7.28, the error 

2 This is the case when the Rodrigues parameters are infinite. The parameter <r 4 is zero when rotating 
through an angle of x radians about an arbitrary line. 
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equations becomes: 

cV c = ei d + K V M* ~ «) + K Pi (e id - £<) (* = 1,2,3) (7.37) 

where the subscript c denotes commanded input to the controller and the subscript d 
denotes the desired value of the Euler parameter. Using the values of £t c and Equation 7.32, 
one can calculate u>. By formulating the dynamical equations of motion in such a way that 
the set of generalized velocities includes the angular velocity terms u>, (e.g. u, = u>, i = 1, 
2, 3) one can solve for the torques by the computed torque method described in Chapter 5. 

7.3 Design Criteria In Three Dimensions 

The experiments conducted in this research were carried out in two dimensions. Obviously, 
space robots that work and move in space will have to be designed to operate in three 
dimensions. This section discusses some of the issues involved in operating a free-flying 
space robot in three dimensions. 

An important issue is the number of arms and degrees of freedom per arm that should 
be incorporated into a space robot. Manipulating an object requires at least six DOF 
(degrees of freedom). If multiple arms cooperate, then each arm is not required to pos- 
sess six DOF. For example, in the plane, two two-link SCARA arms can manipulate a 3 
DOF floating target. Redundancy can be put to good use only at the cost of increased 
mechanical and computational complexity. Also, exerting large torques at the tips of the 
manipulators requires high gearing and large motors. This problem can be reduced by us- 
ing the mechanical advantage available with multiple arms. Using a pair of four DOF arms 
along with a simple three DOF arm would constitute an eighteen DOF robot. Although 
controlling an 18 DOF system seems very difficult with existing computers, many tasks 
could decouple the control problem. For example, if one arm was attached to the space 
station, its motors along with three reaction wheels could control attitude while the other 
arms manipulated a target. The motion of the arms could be treated as disturbances on 
attitude control, thus decoupling the two control systems. This is equivalent to the work 
done in controlling mini-manipulators on a flexible base [19]. 

In addition to manipulating an object, the arms can be used to relocate/reorient the 
robot. It can be shown that one arm possessing six degrees of freedom is sufficient to 
perform a “pushoff” or “catch” task while connected to a structure through a ball joint 3 , 
Angular momentum about the ball joint is constant, since no external forces or torques are 
imparted on the vehicle about that point. However, forces are transmitted through the ball 

3 The idea of using only one arm for locomotion was suggested to the author by Prof. DeBra of Stanford 
University. 
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joint, so that upon release, one can achieve a desired momentum state. One disadvantage 
of this approach is that large joint torques may have to be applied by the arm to bring the 
system to rest. 

The two-arm approach demonstrated in this thesis easily generalizes to three dimen- 
sions. With two two-link arms, the robot can control its momentum in a plane. With the 
aid of one more momentum wheel to initially align that plane in a desired direction, the 
robot can effectively control linear momentum in three dimensions. Angular momentum 
control in three dimensions requires some modifications to the arms, since a screw motion 
is not possible with planar SCARA arms. 

Locomotion tasks are easier to perform in space than on earth. In the absence of 
gravity, the robot does not become unstable when one or more arms (legs) release from 
the bar. Using three legs to crawl along a truss structure can be easily accomplished by 
using a combination of endpoint control to reposition the tips and momentum control to 
move the mass center. Leaping from one point to another can be done in three dimensions 
as was demonstrated in two dimensions. The only concern is what would happen if the 
robot missed the destination. One possibility is to have a tether attached to the space 
structure at the point of release. If the maneuver was successful, a computer onboard the 
space station could release the lock, and the robot could reel in the tether. Otherwise, the 
robot could reel itself back to the original starting point and try again. Another option is 
to use thrusters for midcourse corrections. 



Chapter 8 


Conclusions 


This Chapter is divided into two sections: A summary of the research presented in this 
dissertation, and recommendations for future research. Some of the generic contributions 
of the work are noted in Chapter One. 


8.1 Summary 

This thesis comprised an experimental study of the issues involved in thrusterless locomo- 
tion for space robotic systems. Two major topics were addressed: design and implementa- 
tion of dynamic controllers, and use of strategic control to carry out task level commands. 

Momentum Control A new method for controlling the dynamic behavior of a rigid- 
body system was developed. Called system momentum control , this method allows one to 
specify momentum trajectories instead of Cartesian-space or joint-space trajectories. This 
was shown to be very useful in controlling systems whose plant dynamics changed abruptly 
due to kinematic constraints. The feedforward portion of the controller allows for smooth 
trajectory following and improved performance, while the feedback portion allows errors to 
decay exponentially. This approach is amenable to systems with limited actuator authority 
that could saturate with large feedback gains. 

Current space robotic design and research has focused primarily on issues of object 
manipulation. Thus, one assumes that the objects are in the workspace of the manipulators. 
For fixed automation in a structured environment, this assumption may not constrain the 
effectiveness of the robotic system. However, in space, a fundamental limitation is the 
ability to overlap the task’s location with the robot’s workspace. Designing mobility into 
the task specification instead of constraining the workspace increases the robot’s usefulness. 
The research presented here focuses on the issues of mobility and locomotion. 
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A new approach to space robot locomotion was presented which obviates propulsion 
as the primary means of imparting momentum to the robot: Instead of using thrusters, 
judicious use is made of the arms to transfer momentum to the robot from various struc- 
tures in the environment. A multi- arm robot can push off or grab onto a structure, thereby 
imparting momentum to itself. This cooperative two-arm maneuver would be exceedingly 
difficult to perform by manually controlling the arms because of the nonlinearities in the 
plant and the time-critical path that must be followed. Leaping from one location to an- 
other introduces complexities in dynamics and controller design as the robot’s configuration 
changes from closed-kinematic chains to open-kinematic chains. By focusing on controlling 
momentum, a configuration-independent quantity, system momentum control allows the 
robot to leap precisely from one place to another while accounting for nonlinear forces and 
kinematic constraints. 

Combining system momentum control with operational- space control allows for ad- 
ditional capabilities such as crawling. Combining different forms of locomotion such as 
leaping and crawling reduces that accuracy needed by a single aspect in the overall maneu- 
ver. For example, to relocate the robot to within 10 mm over a 50 m leap would require 
an accuracy in pushoff angle of 0.2 mrad without midcourse correction. If the target area 
is large enough so that the robot can land 200 mm away and crawl to the desired location, 
the initial accuracy required is reduced by a factor of 20. This may not always be feasable, 
but should be considered when designing the robot and the environment in which it will 
work. 

Strategic Control A strategic controller allows for a higher level of abstraction in formu- 
lating and implementing desired tasks , instead of commanding desired states . Implemented 
as a finite-state machine, the strategic controller provides a mechanism for switching be- 
tween synchronous dynamic controllers based upon asynchronous events. Combining a 
strategic controller with a dynamic controller proved to be a very powerful method for 
specifying tasks that switch between various control laws during discrete changes in kine- 
matic constraints. This method is also compatible for manipulation tasks and thus fits into 
an overall framework for combining the various capabilities of a space robot. 

A two-arm free-floating robot was constructed which successfully demonstrated many 
aspects of thrusterless locomotion. By floating on an air-bearing, the robot simulates the 
zero-g drag-free environment of space in two dimensions. The addition of a momentum 
wheel and special end-effectors allows the robot to relocate and reoriented itself, without 
thrusters, in a precise and accurate manner. This can be used to good advantage in space, 
where the cost of propellant, a finite resource, will ultimately determine the usefulness of 
mobile robots. Other benefits from reduced thruster usage include a cleaner environment, 
higher reliability and reduced cost. 
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Although many new features were incorporated into the current-generation space robot, 
some problems still remain. The air gap beneath the vehicle is only 50.8 fi m thick, so dust 
particles from the room cause localized grounding of the base plate. It is very difficult to 
remove all dust and other small particles from a large surface without moving to a clean 
room enviro nm ent. An electrostatic air cleaner was installed to remove particles down to 
0.01 micron di am eter, but this approach has led to mixed results. Therefore, experiments 
involving gross motions such as leaping across the table are difficult to perform repeatably. 

The current ethernet communication link is via a fiber optic cable. Even with one 
vehicle, the cable often gets fouled around the vehicle. This situation will be exacerbated 
when multiple vehicles cooperate together. The obvious solution is to install wireless 
ethernet transceivers on the robots. This technology will become available in the near 
future. 

Lastly, heavy current usage on the batteries greatly reduces shelf life and increases 
internal resistance. NiCd batteries perform best when supplying a small steady current. 
This is not the case during a pushoff maneuver, when peak current is demanded for short 
periods of time. Two possible solutions to this problem are gearing the motors to increase 
the effective torque or installing higher power density batteries, such as sodium sulphur 
batteries. 

8.2 Recommendations for Future Research 

No piece of research is complete in itself. It is hoped that this work will provide some 
answers as well as raise new questions to many of the problems associated with space 
robot locomotion. This section explores some possible extensions for future research. 


8.2.1 Enhancements to the Robot 

The design and construction of the free-flying robot is a continuing process. Several en- 
hancements should be made to the improve its performance and capabilities. The most 
important component that should be added is a vision system. This will allow for accurate 
vehicle position at all times, not just when the robot is in contact with the bar. Also, accu- 
rate endpoint position information will allow object manipulation tasks to be incorporated 
with leaping and crawling. 

The difficulties mentioned above need to be addressed. The fiber-optic link should be 
replaced with a wireless link as the technology becomes available. A different regulator and 
flow meter to allow greater flow rate to the air bearing will allow a larger air gap, reducing 
the possibility of grounding. New batteries and power converters will allow for a second 
CPU board and more digital electronics. 


Cl- A. 
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8.2.2 Adaptive Control and System Identification 

All computed torque schemes are model referenced, requiring knowledge of system inertial 
parameters. While this is a reasonable assumption for a baseline design, large changes m 
inertial parameters could affect performance. For example, if the robot were to fetch a large 
tool, the changes in center of mass location could cause sufficient errors in the control law 
for it to miss the target. The robustness of the system would be improved if an adaptive 
identification of inertial parameters were incorporated into the control law. 


8.2.3 Locomotion in Three Dimensions 

The methods presented in this thesis readily extend to three dimensions. However, without 
experimentation, it is unclear how well these ideas can be implemented on a free-flying 
robot. Some technical issues that need to be addressed are: 

1. Three dimensional sensing systems that will locate the robot, endpoint, and target 
position and orientation in real-time. Although three dimensional vision systems do 
exist, they are significantly more difficult to implement than planar systems. Also, 
vision systems tend to give very poor velocity information, because pixel information 
is quantized. Combining low cost INS sensors such as laser gyros with differential 
GPS could provide centimeter position information with smooth velocity estimates. 

2. The exponential rise in performance for microprocessors has not been paralleled by 
a similar rise in performance for space-qualified processors. For a variety of reasons, 
including limited market and bureaucratic inertia, industry has been slow to imple- 
ment the next-generation 32 bit space-qualified computer. This could be one of the 
major impedements to testing many new ideas and algorithms in space. Current 
space-qualified microprocessors are one to two orders of magnitude too slow, with 
onboard memory capabilities four to ten times too small. 


8.2.4 Optimal Trajectory Generation 

The fifth-order momentum trajectories were generated based upon the heuristic argument 
of modelling the robot as a point mass and accelerating that point in a desired direction. 
It is possible that another trajectory, having the same final momentum state, would be 
better. This would involve solving the Hamilton-Jacobi equations for a nonlinear system 
with limited actuators and geometric constraints. Because the problem is not convex, 
there is no guarantee of achieving global minimization. One of the features of computed 
torque is that it is trajectory-error based, so that any twice-differentiable function can be 
incorporated into the control law . - 
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Another issue that needs further study is finding trajectories to take advantage of the 
ability of the robot to reorient itself without changing its angular momentum. As was 
shown in Chapter 7, the change in attitude is related geometrically to the path of shape 
changes. Finding optimal paths that cause large attitude changes for “small” shape change 
would be useful. It may turn out that reaction wheels or CMG’s would be more practical for 
orientation control than relying on shape changes. On the other hand, some combination 
of shape changes and reaction torques may be better for some problems. This is a very 
fertile area with interesting theoretical as well as practical implications. 


8.2.5 Multiple Vehicle Cooperation 

Many tasks that are difficult to perform with one robot may be easier to accomplish with 
two or more robots. For example, assembling a large space structure composed of long 
slender beams is not amenable to being accomplished by only a single robot. Coordinating 
and managing multiple robots in an efficient manner is an interesting problem. Issues that 
need to be addressed include collision avoidance, distributive control strategies, parallel 
processing, and momentum management of multiple vehicles. For example, if two or more 
vehicles were to attach themselves to a free beam, it might be desirable to achieve zero net 
change in the beam’s momentum. Using system momentum control, the momentum of each 
vehicle could be controlled at “pushoff” time so that when both robots attach themselves 
to the beam, the net momentum of the system is zero. 


Appendix A 


Calibration 


Automated and semi-automated calibration procedures were developed for the joint angle 
sensors, pseudo-rate joint signals, angular rate sensor, and motor torque outputs. These 
routines collect data to be processed by Pro-Matlab, and provide a nice environment for 
data collection and analysis. This appendix describes how the various components of a 
free-flying robot are measured and calibrated. 


A.l Joint Angle Sensor Calibration 

Calibrating joint angles on a free-flying robot is considerably harder than on a fixed-base 
robot. Most robotic applications desire knowledge of the manipulator endpoint, and so 
by fixing the endpoint in inertial space, one can back out the joint angles (assuming no 
redundancy) from inverse kinematics. For a free-flying robot, fixing the endpoint of the 
arms does not uniquely determine the arm’s joint angles, since there are extra degrees of 
freedom in the base. 

To circumvent this problem, a calibration fixture was constructed that could be attached 
to the robot in a repeatable manner. The fixture consisted of 32 holes drilled in a 1/8” 
aluminum sheet covering most of the workspace. A steel peg is placed through the tip of 
the arm to center it over a desired hole. Originally, the plate was designed such that only 
one joint at time changed angle as the arm moved in an arc over a subset of calibration 
holes. Only when the plate is attached perpendicular to the robot’s axis of symmetry would 
this happen. Therefore, before any data was taken, one could be sure that the calibration 

plate was attached in a known and repeatable manner. 

The joint angle calibration program prompts the user to position the tip in a numbered 
hole. It then uses inverse kinematics to calculate the actual joint angles (shoulder and 
elbow) for each location. A user specified number (usually 250) of measured points are 
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averaged at each location, and stored in a file with the actual measurement. A linear 
regression least-squares fit is performed to yield scale factor and offset. To further reduce 
the nonlinear effects of the sensor, a forth order polynomial fit is performed on the output 
of the least-squares fit. 




Figure A.l: Joint Calibration 

The /east-squares data was filtered through a fourth order polynomial to 
further reduce errors due to noniinearities in the sensor. 


w 




A. 2 Joint Velocity Sensor Calibration 

After all the joint angles are calibrated, joint velocity calibration is performed. This is 
a completely automated procedure, whereby the arms are slewed through the workspace 
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under PD control while the velocity signals axe integrated. By comparing the integrated 
velocity signal with the measured slew angle, the scale factor and offset for the pseudo- 
differentiated rate signal can be computed. 


A.3 Model Parameter Measurements 


Control methods such as computed torque and momentum control are model based and 
depend upon an accurate knowledge of the dimensional parameters of the plant. These 
include mass, center of mass location, joint location, and moment of inertia for each body. 
The masses of each body were weighted on an electronic scale to approximately 0.1 gram, 
while all lengths were machined to a tolerance of ±0.005 inches. The location of the mass 
center was determined by balancing the object on two knife edges, and measuring the 
resulting force under each end. A simple static force/torque equilibrium calculation yields 
the position of the mass center. 

Inertias for all the bodies except the base were measured with a trifilar pendulum, 
described in Appendix A of [13]. Briefly, the apparatus consists of an aluminum triangular 
plate, suspended from the ceiling by three wires. After the instrument is calibrated, a test 
object of mass m is placed on the plate such that the object’s mass center and the plate s 
center axe coincident. For small displacements, the period of oscillation, u>, is related to 
the moment of inertia, /, by the equation: 


u = 



where l is the distance from the center of the plate to the suspension wires, L is the length 
of the support wires, and g is the acceleration of gravity. A good estimate of the inertia is 
obtained by averaging over then periods of oscillation. This procedure was automated by 
Stan Schneider and Larry Pfeffer, and is described in [29]. Comparison between measured 
inertias and those predicted by the CAD software package IDEAS [33] agree to 1%. 

The base inertia was measured by a different means because its size and weight were 
too large for the trifilar pendulum. The base was floated with the arms set in a nominal 
tuck position. The moment of inertia of the system about the mass center can be derived 


from: 



where 0 and 0 are the angular position and rate of the base, T is the torque applied by 
the momentum wheel motor, and I is the inertia of the system. Knowing the inertia of the 
arms, one can derive the moment of inertia of the base. Using this method, it is estimated 
that the base inertia is known to about 7%. 



Appendix B 


Air Bearing 


The following is a simplified derivation of an air bearing* Readers interested in a more 
detailed treatment on the subject should see Rehsteiner [25]. 

When solving most fluid mechanics problems, certain assumptions are made in order 
to solve the nonlinear Navier-Stokes equations. In this problem, we wish to derive the 
equations for an air bearing. The geometry of the problem consists of a source between two 
flat plates of radius r 0 , with the top plate having a plenum radius r, . It is assumed that the 



Figure B.l: Air Bearing 

fluid is Newtonian (/z 0 = constant) and incompressible ( p 0 = constant). Furthermore, based 
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on the geometry, we assume axisymmetric steady flow. These assumptions correspond to 

— = 0 steady flow 
dt 

u z = ug - 0 

0 

— = 0 axisymmetric flow 
dP 

— - = 0 neglect gravity 
dz 


With these assumptions, the incompressible Navier-Stokes equations [35] can be for- 
mulated in cylindrical coordinates in the radial direction as: 


U T 


du r 

dr 
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d 2 u T 1 du T 
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Equation B.l, the conservation of mass equation, simplifies to: 


= 0 


(B.3) 

(B.4) 


where /(z) is an unknown function of z yet to be determined. Substituting this result into 
Equation B.2, the momentum equation, and simplifying yields: 

l_dP f(zf VQ pftz) 

Po dr r 3 r dz 2 

The first term on the right corresponds to momentum transfer due to inertial forces. Be- 
cause the flow is laminar and slow, this term can be neglected, leaving just the second and 
dominant term. Physically, the equation relates the radial pressure drop due to the viscous 
force on the fluid. Integrating the above equation with respect to z yields: 


-^T +Ci W* +C2(r)=i r /w < B - 5 ) 

po Or L r 

Where Ci(r) and C 2 (r) are two unknow functions of r. If one now considers the case of a 
stationary base, the boundary conditions become /(A/2) = /(—A/2) = 0 which is the “no 
slip” condition at the two surfaces. Recall that h is the gap or thickness of the air-bearing. 
Substituting the boundary conditions into Equation B.5 gives: 


Ci(r) 
Ci{ r) 

/(*) 


0 

-1 dPh 2 
2po dr 4 
r dP 2 _ h?_ 
2po dr 4 
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The last equation is a function of z only, imposing the condition: 

t^J- = Cz a constant 

dr 

P(r) = C 3 hi(r/r 0 ) ri<r<r 0 
For the air bearing to support the weight of the robot 1 
mg = j P d A 

= f [ C 3 ln(r/r o )r<ird0+ nC 3 r^ln(r,/r 0 ) 
JO Jri 


= *C 3 — 


r? - rl 


from which we can solve for the constant C 3 . The velocity profile can now shown to be: 


u r 


mg 

Ho* (rf - rl)r 



h?_ 

4 


One last quantity of interest is Q, the flow rate. This can be calculated as: 

/ h/2 r2r 

I u r rdBdz 
-h/2 JO 

_ (B.6) 

6mo 

Equation B.6 shows why it is so difficult to increase the gap height of the air bearing: the 
flow rate is proportional to the cube of the gap thickness. 


1 It is also assumed that the pressure under the plenum is constant with a value of P{r t ) 
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